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This  thesis  presents  a  novel,  practical,  and  theoretically  sound  kinematic  control 
strategy  for  serial  redundant  manipulators.  This  strategy  yields  repeatability  in  the 
joint  space  of  a  serial  redundant  manipulator  whose  end  effector  undergoes  some 
general  cyclic  type  motion.  This  is  accomplished  by  deriving  a  new  inverse  kinematic 
equation  that  is  based  on  springs  being  theoretically  or  conceptually  located  in  the 
joints  of  the  manipulator  (torsional  springs  for  revolute  joints,  translational  springs 
for  prismatic  joints). 

Previous  researchers  have  also  derived  an  inverse  kinematic  equation  for  serial 
redundant  manipulators.  However,  to  the  author's  knowledge,  the  new  strategy  is  the 
first  to  include  the  free  angles  of  torsional  springs  and  the  free  lengths  of 
translational  springs.  This  is  important  because  it  ensures  the  repeatability  in  the 
joint  space  of  a  serial  redundant  manipulator  whose  end  effector  undergoes  a  cyclic 


type  motion.  Numerical  verification  for  repeatability  is  done  in  terms  of  Lie  bracket 
condition. 

Choices  for  the  free  angle  and  torsional  stiffness  of  a  joint  (or  the  free  length  and 
translational  stiffness)  are  made  based  upon  the  mechanical  limits  of  the  joint.  For 
instance,  the  free  angle  of  a  joint  is  that  angle  which  is  midway  between  joint  limits. 
Joint  stiffnesses  are  chosen  so  that  the  most  dexterous  joint  is  the  most  pliable,  and 
so  that  the  least  dexterous  joint  is  the  stiffest.  This  strategy  helps  to  keep  the  joints 
of  the  manipulator  away  from  their  respective  joint  limits. 
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CHAPTER  1 
INTRODUCTION 

There  is  an  increased  interest  in  using  manipulators  with  seven  or  more  degrees 
of  freedom  because  of  the  dexterity  limitation  of  current  six  degree  of  freedom 
manipulators.  For  example,  the  limited  ranges  of  motion  of  the  joints  and  torque 
limits  on  motors  restrict  the  movement  of  a  six  degree  of  freedom  manipulator. 
Because  of  these  considerations,  it  is  desirable  for  a  general  purpose  robotic 
manipulator  to  have  more  than  six  degrees  of  freedom. 

Redundant  manipulators  have  more  degrees  of  freedom  than  is  required  for  a 
task.  For  example,  a  seven  degree  of  freedom  manipulator  is  redundant  for  the  task 
of  positioning  and  orienting  the  end  effector  (three  degrees  of  freedom  are  required 
for  position  and  three  are  required  for  orientation).  The  extra  degree(s)  of  freedom 
of  a  redundant  manipulator  may  be  used  to  satisfy  many  supplementary  criteria  such 
as  keeping  the  joint  variables  within  their  physical  limitations,  providing  greater 
dexterity,  placing  the  joint  torques  close  to  the  midpoint  of  joint  torque  limits,  and 
minimizing  kinetic  energy,  singularity  avoidance,  obstacle  avoidance,  etc. 

In  most  work  on  serial  redundant  manipulators,  the  forward  velocity  equations 
are  frequently  written  in  a  Jacobian  form,  and  the  Moore-Penrose  pseudo-inverse  of 
the  Jacobian  is  used  to  give  the  minimum  Euclidean  norm  solution  [1].  The  use  of 
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this  procedure  gives  a  solution,  subject  to  the  condition  of  minimum  norm,  in  cases 

where  all  the  joints  are  either  revolute  or  prismatic,  or,  indeed,  for  the  case  of  robots 

where  the  joints  are  all  of  the  same  type.  However,  the  use  of  the  Moore-Penrose 

pseudo-inverse  has  much  more  serious  consequences  when  the  serial  redundant 

manipulator  consists  of  a  mix  of  joint  types.  If  there  are  both  revolute  and  prismatic 

joints  present,  for  example,  minimizing  the  Euclidean  norm  lacks  formal  meaning, 

and  the  result  so  obtained  is  heavily  dependent  on  the  unit  of  length  used  to  describe 

the  robot  structure. 

Indeed,  it  is  also  possible  to  arrive  at  other  solutions  by  superposing  other 

allowable  motions  of  the  manipulator  that  do  not  entail  end  effector  motion  of  any 

kind  in  addition  to  the  Moore-Penrose  pseudo-inverse  solution  [2]. 

Whitney  [3]  applied  the  weighted  pseudo-inverse  solution  to  the  inverse  kinematic 

problem  for  redundant  manipulators.  This  differs  from  the  Moore-Penrose  solution 

in  that  the  sum  of  squares  of  actuator  velocities  is  a  weighted  sum  (a^e2  +  a2«SG22 

+  ...  +  anS9n2,  where  ax,  a2,  ...,  an  are  positive  numbers  chosen  by  the  user).  The 

Moore-Penrose  pseudo-inverse  control  or  the  weighted  pseudo-inverse  control  may 

drive  the  robot  into  an  undesired  manipulator  configuration.  Using  these  techniques, 

the  manipulator  configurations  themselves  are  not  reproduced  when  an  end  effector 

performs  a  cyclic  motion  (the  joint  displacements  are  not  cyclic).  Several  researchers 

have  identified  the  repeatability  problem  of  redundant  manipulators  [1,4,5]. 

This  thesis  describes  a  general  pseudo-inverse  solution  to  the  inverse  kinematic 

problem  for  redundant  manipulators.  This  solution  is  one  that  determines  a  suitable 


set  of  actuator  velocities  (or  increments)  which  minimize  a  general  quadratic 
function:  au<5e2  +  a^ae/  +  ...  +  a^se,2  +  a^se^ej  +  a23<5G2<5e3  +  . . . 
+  a(n.1)n<5e  n.^e,,.  The  coefficients  au,  ay  are  not  constants  (they  change  as  the 
manipulator  moves).  In  order  to  specify  au,  a^  (or  [A]  from  the  matrix  expression,  6eT 
[A]  56),  this  work  bases  its  foundation  on  a  pseudo-elastic  potential  energy  function. 
In  addition  to  actuator  stiffnesses  (torsional  or  translational),  this  new  function  also 
includes  free  angles  of  torsional  springs  and/or  free  lengths  of  translational  springs. 
This  formulation  yields  a  control  strategy  that  facilitates  repeatability  in  the  joint 
space  of  a  manipulator  whose  end  effector  undergoes  a  cyclic  type  motion. 


CHAPTER  2 
ANALYSIS  OF  SERIAL  REDUNDANT  MANIPULATORS 


This  chapter  contains  a  theoretical  development  and  discussion  of  the  new 
control  strategy. 

2.1  Introduction  to  Pliicker  Line  Coordinate 

Because  lines  (screws)  will  be  used  in  subsequent  sections,  a  review  of  line 
coordinates  is  presented  here. 

Two  distinct  points,  IiCx^y^Zj)  and  l2(x2,y2,z2),  determine  a  line  as  shown  in  Fig.  2- 
1.  The  vector  S  can  be  expressed  in  the  form 

S  =  Li  +  Mj  +  Nk,  (2.1) 

where 

L  =  x2-xx,       M  =  y2-yx,       N  =  z^z,,  (2.2) 

are  defined  as  the  direction  ratios.  The  direction  ratios  (L,M,N)  are  related  to  the 
distance  between  the  two  points  by 

V  +  M2  +  N2  =  |S|2.  (2.3) 

Let  r  represent  a  vector  to  any  arbitrary  point  on  the  line.  The  vector  (r-ij)  will 
be  parallel  to  S  and  therefore  the  equation  of  the  line  may  be  written  as 

(l-I,)  x  S  =  0.  (2.4) 

This  equation  can  be  expressed  in  the  form 


r  x  S  =  So, 
where 
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(2.5) 


So  =  h  *  S,  (2.6) 

is  the  moment  of  the  line  about  the  origin  and  is  origin  dependent.  The  vectors 
(S;So)  which  must  satisfy  the  orthogonality  condition 

S-So  =  0,  (2.7) 

are  the  Pliicker  coordinates  of  the  line  [6,7].  (S;So)  are  homogeneous  coordinates 
since  from  eq.  (2.5)  the  coordinates  (kSjkSo)  where  k  is  a  non-zero  scalar  determine 
the  same  line.  The  vectors  (S;S_o)  may  be  written  in  terms  of  their  components  as 
(L,M,N;P,Q,R). 


Fig.  2-1:   Development  of  Pliicker  Line  Coordinates 


A  wrench  on  a  screw  will  be  represented  as  a  general  "ray"  that  is  assigned  an  R6 
vector  w  =  [f:  tt^Y,  where  f  is  a  force  vector  in  the  direction  of  the  wrench  and  m,, 
is  a  moment  vector  referenced  to  the  origin.  Ray  coordinates  for  screws  (direction 
vector  first)  are  assigned  lower  case  labels.  This  ordering  of  screw  coordinates  is 
based  on  Plucker's  definition  of  a  ray,  which  is  a  line  formed  from  the  join  of  two 
points.  However,  a  twist  on  a  screw  will  be  represented  using  two  different  formats, 
one  based  on  the  ray,  d  =  [<Sr;  SJ],  and  the  other  based  on  a  general  "axis"  that  is 
assigned  an  R6  vector,  D  =  [6&  SR].  Axis  coordinates  for  screws  (direction  vector 
last)  are  assigned  upper  case  labels.  Plucker  defined  the  axis  to  be  dual  to  the  ray, 
and  axis  is  thus  a  line  dually  formed  from  the  meet  of  two  planes. 

For  a  twist,  Si  and  S&  both  represent  the  direction  of  an  infinitesimal  rotation, 
and  Sr  and  5R  both  represent  an  infinitesimal  displacement  of  the  origin.  Therefore, 
the  axis  and  ray  representations  of  the  same  twist  are  related  by 


'si  ' 

= 

"03     i3- 
I3     03. 

SR 

which  can  be  more  conveniently  written  as 

8  =  [A]D ,  (2.8) 

where  [A]  is  the  symmetric  6x6  matrix  above.  Since  [A]"1  =  [A],  a  reversal  in  the 

ordering  of  this  same  transformation  yields 

D   =  [A]d.  (2.9) 

The  transformation  described  by  eq.  (2.8)  and  eq.  (2.9)  is  an  example  of  a  more 


7 
general  transformation,  defined  as  a  correlation,  that  maps  an  axis  to  a  ray  (or  a  ray 
to  an  axis).  It  is  important  to  distinguish  this  transformation  from  a  collineation, 
which  maps  a  ray  to  a  ray  (or  an  axis  to  an  axis). 

2.2  Development  of  the  New  Kinematic  Control  Strategy 
We  consider  an  n  degree  of  freedom  manipulator  illustrated  in  Fig.  (2-2),  with 
joint  coordinates  e; ,  i  =  1,  2,  .  . . ,  n,  and  a  task  described  by  m  task  coordinates  ri 
,  j  =  1,  2,  .  . . ,  m  (  m  <  n  )  .  Let  the  kinematic  transformation  from  the  joint  space 
to  the  task  space  be  given  by 

r  =  f(G),  (2.10) 

where  e  =  [elf  e2,  .  .  .  ,  en]T  and  r  =  [r„  r2,  .  .  .  ,  rm]T  are  the  joint  and  task 
coordinate  vectors  respectively.  The  superscript  T  denotes  the  transpose.  Eq.  (2.10) 
will  be  assumed  to  be  continuous  and  differentiable  up  to  the  second  order  in  the 
entire  task  space.  Differentiating  eq.  (2.10)  with  respect  to  time  yields 

L  =  J(e)e    ,  (2.11) 

where  i_  =  dr/dt  e  Rm  (m-dimensional  Euclidean  space),  e  =  de/dt  e  Rn  ,  and 
J(e)  =  3f(e)/3e  e  R"""  (the  set  of  all  m  x  n  real  matrices).  The  matrix  J(e)  is  called 
the  Jacobian.  From  eq.  (2.11),  we  see  that  the  Jacobian  is  simply  a  linear 
transformation  that  maps  the  joint  velocity  in  Rn  into  the  task  velocity  in  Rm. 

The  end  effector  is  required  to  undergo  an  infinitesimal  twist  relative  to  ground. 
Then  an  alternative  expression  of  eq.  (2.11)  for  the  forward  instantaneous  motion  can 
be  written  in  the  form 


fv 


'V 


\J     $5 


Fig.  2-2  :  Kinematic  Model  of  n-Jointed  Serial  Manipulator 


D   =    Z6B&,  (2.12) 

i=l 

where  D  is  the  axis  coordinates  of  the  twist,  line  $,  is  assigned  axis  coordinates  St , 
and  where  56;  is  an  infinitesimal  rotation  of  the  ith  revolute  axis.  This  can  be  more 
conveniently  written  as 

D  =  [J]  59,  (2.13) 

where  [J]  is  the  m  x  n  Jacobian  matrix  whose  column  is  Sj .  It  is  observed  that  [J]  can 
be  obtained  by  eq.  (2.11)  or  eq.  (2.13). 

While  the  objective  of  this  work  is  to  develop  a  meaningful  algorithm  that  effects 
the  incremental  displacement  control  of  the  end  effector  of  a  serial  redundant 
manipulator,  it  is  now  assumed  that  at  each  revolute  joint  there  exists  a  torsional 
spring  which  deflects  due  to  the  rotation  of  the  joint.  In  general,  the  torsional  spring 
can  be  thought  of  as  deflected  away  from  its  free  position  angle,  and  it  is  thus 
initially  loaded  with  a  finite  torque,  Tj  at  any  general  configuration.  Consequently,  the 
stiffness  elements  can  be  considered  at  each  instant  to  be  loaded  and  not  to  be  at 
their  free  positions.  Then  this  system  of  springs  would  be  in  static  equilibrium,  with 
an  appropriate  external  resultant  wrench  providing  a  load  necessary  to  balance  the 
finite  torsional  spring  deflections.  In  an  infinitesimal  movement,  the  ith  torsional 
spring  is  considered  to  have  linear  characteristics,  and  t{  in  the  ith  spring  torque  is 
related  to  a  joint  rotation  9;  -  e0i  by 

t;  =  kfo  -  G0i),  (2.14) 

where  9;  -  90i  is  the  difference  between  the  current  and  the  free  position  joint  angle 
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and  kj  is  the  non  zero  ith  joint  stiffness  that  has  dimensions  (FL).  The  eq.  (2.14)  may 

be  more  conveniently  written  as 

e  -  e,  =  [k]"1!,  (2.15) 

where  [k]  is  an  n  x  n  diagonal  matrix  of  joint  stiffnesses. 

The  joint  stiffness,  k;,  can  also  be  a  desired-joint-displacement  stiffness,  which  is 
defined  as  how  stiff  a  joint  is  desired  to  be  while  contributing  to  the  generation  of 
a  known  end  effector  twist.  The  larger  kf  will  cause  less  joint  rotation  while  the 
smaller  k;  will  cause  more  joint  rotation.  Thus  a  proper  choice  of  k;  based  on 
mechanical  limits  of  each  joint  helps  to  avoid  joint  limitation  during  the  specified 
path. 

The  joint  torques,  acting  in  each  joint,  combine  to  give  a  resultant  wrench  (w  = 
[f:  So]1)  given  in  ray  coordinates  at  the  end  effector,  and  this  can  be  expressed  for 
the  ith  joint  as 

t,  =  S,T  w,  (2.16) 

which  can  be  more  conveniently  written  in  the  form  for  all  joints, 

X  =  [J]T  w,  (2.17) 

where  jr  is  the  n-vector  of  the  joint  torques.  Combining  the  stiffness  relationships  in 
eq.  (2.15)  with  eq.  (2.17)  gives  the  vector  of  joint  motions 

G  -  e,  =  [kjW  w .  (2.18) 

Multiplying  eq.  (2.18)  by  [J]  and  rearranging  eq.  (2.18)  yields 

w  =  ([JpnJf)-1  [J]  (e  -  a,).  (2.19) 

This  can  be  solved  provided  that  [J]  is  of  full  rank.  Kinematic  singularities  occur 
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when  [J]  loses  rank.  Such  kinematic  singularities  are  intrinsic  to  the  structure  of  the 
mechanism.  Taking  the  derivative  eq.  (2.18)  yields 

se  =  [k]4[J]T  6w  +  \k]-1[SJ]T  w,  (2.20) 

The  second  term  of  the  right-hand  side  of  eq.  (2.20)  goes  to  the  left-hand  side, 

<Se  -  [k]1[6J]T w  =  [k)l[J]T  5w  (2.21) 

The  second  term  on  the  left-hand  side  of  eq.  (2.21)  contains  «S9  in  [6J]T.  In  order  to 
extract  56  from  [5J]T,  the  following  will  be  accomplished.  The  ith  component  (i  = 
1,  2,  ...,  n)  of  the  vector  [<5J]T  wean  be  expressed  in  the  form: 

*  m      n 

([5J]Tw)(   =  z  (s{(#rIfieI)/(3elaeJ)^ 

1=1  J  =  l 

=  £fi4*%  (2-22) 

where 

m 

In  vector  notation,  eq.  (2.22)  becomes 

[6J]Tw=  [T]SQ.  (2.23) 

Since,  by  hypothesis,  eq.  (2.10)  is  continuous  and  differentiable  in  the  task  space,  it 
can  be  said  that 

From  the  above  equation,  it  follows  that  [r]  is  an  n  x  n  symmetric  matrix  (ry  =  iv). 
Substituting  eq.  (2.23)  into  eq.  (2.21)  yields 

(I  -  [k]'1[r])6fi  =  [kYVY  6vf.  (2.24) 

Letting 

m  =  i  -  [knn 
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then  eq.  (2.24)  becomes 

[T]«59  =  WW  «w,  (2.25) 

where  [T]  is  an  n  x  n  matrix  that  consists  of  the  joint  angles,  eif  the  free  angles  of 
torsional  springs,  G0j,  an  external  wrench,  w,  and  spring  stiffnesses,  ks,  i  =  1,  ...,n. 
Eq.  (2.25)  can  be  solved  provided  that  [Tj  is  of  full  rank  (otherwise  an  algorithmic 
singularity  occurs).  Thus, 

69  =  [T]1[k]1[J]T  5w.  (2.26) 

Letting 

[A]  =  [k][T], 
which  is  an  n  x  n  symmetric  matrix  and  premultiplying  the  left  and  right  sides  of  eq. 
(2.26)  by  [J]  yields 

[J]  69  =  [J][A]1[J]T  <5w.  (2.27) 

Substituting  eq.  (2.13)  into  eq.  (2.27)  yields 

D   =  [J][A]*W  5w,  (2.28) 

which  can  be  expressed  in  the  form 

D  =  [C]6w,  (2.29) 

where  [C]  =  [JHA]"1^]1"  is  an  m  x  m  symmetric  compliance  matrix  that  relates  the 
end  effector  external  wrench  to  its  corresponding  twist.  Provided  that  [T]  and  [J]  are 
both  full  rank,  then 

6w  =  [C]1!) ,  (2.30) 

relates  the  necessary  deflection  twist  of  the  end  effector  relative  to  ground  to  the 
incremental  change  in  resultant  wrench. 
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In  the  problem  of  the  inverse  kinematics  of  the  serial  redundant  manipulator,  it 

is  necessary  to  determine  the  joint  increments  explicitly.  The  elastic  compliance 

relation  of  eq.  (2.28)  can  be  used  to  accomplish  this  objective.  Substituting  eq.  (2.30) 

into  eq.  (2.26)  yields  the  general  pseudo-inverse  equation: 

60  =  [Al-WtfJHA^tJf^D 

=  [AppTCQ*.  (2-31) 

Application  of  the  general  pseudo-inverse  solution  depends  upon  the  knowledge 
of  [A].  Because  [A]  is  manipulator  configuration  dependent,  it  must  be  calculated 
every  time  eq.  (2.31)  is  to  be  solved. 

Eq.  (2.31)  can  be  also  derived  by  using  the  Lagrangian  multiplier  method. 
THEOREM  2.1.  If  J  is  an  m  x  n  matrix  (m  <  n)  of  rank  m,  then  the  solution  of  the 
equation  D   =  [J] 66  that  minimizes  6GT[A]50  is  given  by 

66  =  [AjW {PIA1-W}-*  •  (232) 

Proof:  We  consider  the  method  of  Lagrange  multipliers.  The  quantity  D    that 
minimizes  69T[A]6e  subject  to  D   =  [J]6G  can  be  found  by  minimizing 

M  =  6eT[A]*e_  +  2XT(D  -  [J]  66), 
where  X  is  an  m  x  1  column  vector  of  Lagrange  multipliers,  and  the  factor  of  2  is 
inserted  for  convenience.  Setting 

dM/dSQi  =  0,  j  =  1,  .  .  .,  n,       dM/dX{  =  0,   i  =  1,  .  .  .,  m, 
we  find 

[A] 69  =  [J]Tk  or  66  =  [Aj-WX.  (2.33) 

D   =  [J]  60.  (2.34) 
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Substituting  eq.  (2.33)  into  eq.  (2.34),  we  obtain 

D  =  [J][A]1[J]TX  or  X  =  ([J][A]1[J]T)'1D ,  (2.35) 

Substituting  eq.  (2.35)  for  X  into  (2.33)  gives  (2.32). 

This  solution,  eq.  (2.31),  ensures  the  repeatability  in  the  joint  space  of  a  serial 

redundant  manipulator  whose  end  effector  undergoes  a  cyclic  type  motion. 

Taking  the  derivative  of  eq.  (2.15)  yields 

59  =  [kj-^r.  (2.36) 

A  further  extension  of  this  analysis  can  be  achieved  by  combining  eq.  (2.13)  and  eq. 

(2.36)  with  eq.  (2.30),  to  obtain 

6w  =  [C]"1  [Jp]"1  SL,  (2.37) 

which  is  the  forward  static  relationship  for  the  serial  redundant  manipulator.    Eq. 

(2.37)  describes  an  onto  mapping  of  joint  torque  increments  into  wrench  increments. 
From  the  above  theorem  2.1,  the  general  pseudo-inverse  equation  minimizes 

<S6T[k][T]6e.  It  is  important  to  recognize  that  potential  energy  is  not  being 

n 

minimized,  i.e.,  pe  =  2  O^k,(0j  -  e0i)2.  Furthermore,  it  will  be  now  demonstrated  that 
the  second  derivative  of  potential  energy,  which  is  denoted  by  <S2pe,  is  also  not  being 
minimized.  Now  the  relationship  between  <5eT[k][T]6e  and  the  second  derivative 
potential  energy  function  will  be  derived.  The  second  derivative  potential  energy  can 
be  written  in  the  form 
<52pe    =  5(DTw), 

=  5DTw+  DT6w  (2.38) 

Premultiplying  the  left  and  right  sides  of  eq.  (2.25)  by  66T[k]  yields 
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seT[k][T]6G    =  $eT[J]T5w, 

=  DT5w  (2.39) 

Substituting  eq.  (2.39)  into  eq.  (2.38)   and  rearranging  eq.  (2.38)  yields 

6eT[k][T]se  =  s^e  -  <SDT  w.  (2.40) 

Because  potential  energy  is  not  being  minimized,  the  use  of  the  control  strategy 
does  not  require  at  any  time  the  robot  to  be  in  a  minimum  (or  maximum)  potential 
energy  state,  i.e.,  where  5pe  =  0.  In  fact  the  robot  whose  end  effector  undergoes 
some  general  cyclic-type  motion  can  begin  in  different  initial  configurations  (different 
initial  joint  angles).  A  theoretical  proof  of  repeatability  is  given  (section  2.4) 
following  a  brief  discussion  of  other  control  strategies. 

2.3  Relationship  of  New  Control  Strategy  to  Previous  Control  Strategies 
In  this  section,  it  is  assumed  that  the  manipulator  system  is  initially  stationary  and 
unloaded.  Consequently,  the  stiffness  elements  can  be  considered  at  each  instant  to 
be  unloaded  or  at  their  free  positions,  i.e.,  0j  =  e0i  and  from  eq.  (2.19)  w  =  0. 
Therefore  from  eq.  (2.20) 

Se  =  [k]VY  Svf.  (2.41) 

Premultiplying  the  left  and  right  sides  of  eq.  (2.41)  by  [J]  yields 

[J]  56  =  [J][k]1[J]T  Svf.  (2.42) 

Substituting  eq.  (2.13)  into  eq.  (2.42)  yields 

D   =  [J][k]-W  6w,  (2.43) 

which  can  be  expressed  in  the  form 
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D   =  [C]<Sw,  (2.44) 

where  [C]  =  [J][k]"x[J]T  is  an  m  x  m  symmetric  compliance  matrix  that  relates  the  end 
effector  external  wrench  to  its  corresponding  twist.  Provided  that  [J]  is  of  full  rank, 
thus 

5w  =  [C]  x£> ,  (2.45) 

relates  the  necessary  deflection  twist  of  the  end  effector  relative  to  ground  to  the 
incremental  change  in  the  resultant  wrench. 

In  the  problem  of  the  inverse  kinematics  of  the  serial  redundant  manipulator,  it 
is  necessary  to  determine  the  joint  increments  explicitly.  The  elastic  compliance 
relation  of  eq.  (2.41)  can  be  used  to  accomplish  this  objective.  Substituting  eq.  (2.45) 
into  eq.  (2.41)  yields  the  weighted  pseudo-inverse  equation: 

se  =  [kYWaWYVY)1® , 

-MW(P*D..  (2-46) 

The  result,  which  is  the  weighted  pseudo-inverse  of  [J],  represents  a  minimization  of 
the  second  derivative  of  elastic  potential  energy  function  <S2pe: 

S^e  =  seT  [k]  fie.  (2.47) 

The  work  here  agrees  with  the  established  work  of  Whitney  [3].  It  is  recognized  that 
eq.  (2.46)  does  not  ensure  repeatability  of  cyclic  motion  of  the  end  effector. 

The  Moore-Penrose  pseudo-inverse  solution  can  also  be  obtained  from  the  eq. 
(2.31)  by  disregarding  the  free  angles  and  the  joint  stiffnesses.  Thus,  both  [k]  and  [T] 
are  identity  matrices  and  from  eq.  (2.31), 
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«e  =  [J]T([J][J]T)1D.  (2.48) 

Eq.  (2.48)  is  the  same  as  that  of  Noble  [8].  The  result,  which  is  the  Moore-Penrose 
pseudo-inverse  of  [J],  represents  a  minimization  of  5eT<Se.  It  is  very  important  to 
note  that  eq.  (2.48)  does  not  ensure  repeatability  of  the  cyclic  motion  of  the  end 
effector.  Furthermore  it  is  not  invariant  with  change  of  unit  of  length  if  there  are 
both  revolute  and  prismatic  joints  present. 

2.4  Discussion  of  Repeatability  Based  Upon  the  Lie  Bracket  Condition 
Shamir  and  Yomdin  [5]  introduce  a  repeatability  condition  of  redundant 
manipulators.  In  order  to  explain  the  repeatability  condition,  they  introduce  7 
definitions  and  4  theorems.  In  this  section  the  definitions  and  the  theorems  are 
discussed  briefly. 

DEFINITION  2.1:  An  n  x  m  matrix  function  P  =  P(e)  such  that  [J][P]  =  1^  will  be 
called  a  local  control  strategy. 

DEFINITION  2.2:  Let  x  =  x(t)(t  e  [0,1])  be  a  continuous  curve  in  the  task  space.  If 
a  =  a(t)  is  a  continuous  curve  in  the  joint  space  such  that  f(a(t))  =  x(t),  then  a  is 
called  a  lifting  of  x. 
DEFINITION  2.3:  Let  r0  be  a  point  in  the  task  space  W.  The  set 

f-Vo)  =  {e  e  *  |  f(e)  =  r0} 
will  be  called  the  fiber  over  r0. 

DEFINITION  2.4:  A  control  strategy  P  is  repeatable  over  U  (open  subset  of  W)  if 
for  any  closed  path  x  in  U,  every  lifting  of  x  determined  by  P  is  a  closed  path  a  in 
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f^U),  and  a  is  never  tangent  to  any  fiber. 

DEFINITION  2.5:  A  control  P  is  repeatable  at  9,,  e  *,  if  for  any  closed  path  x  in  U 

that  passed  through  f(9p),  the  lifting  determined  by  P  and  passing  through  e,,,  is  a 

closed  curve  in  $. 

DEFINITION  2.6  [9,  p.  88;  10,  p.  123]:  An  m-dimensional  distribution  D  on  a  set  A 

in  R"  is  a  law  that  assigns  to  each  point  e  e  A,  a  unique  linear  m-dimensional 

subspace  De(of  Rn),  with  origin  at  e.  It  is  sometimes  also  called  an  m-vector  field 

[11,  p.  145]. 

DEFINITION  2.7:  An  integral  surface  S  for  an  m-dimensional  distribution  D  is  a 

smooth  m-dimensional  surface  S  c  Rn,  such  that  at  each  point  e  e  S,  the  tangent 

space  to  S  is  exactly  the  linear  m-dimensional  space  D§  assigned  by  the  distribution. 

THEOREM  2.2:  Let  P  be  our  control  matrix,  which  we  assume  to  be  a  smooth 

function  in  an  open  subset  *  of  the  joint  space.  Let  U  e  f(#)  be  a  simply_connected 

region  of  the  task  space.  Then  the  control  is  repeatable  for  any  closed  paths  lying  in 

U,  if  and  only  if  the  following  condition  (LBC)  holds. 

LBC:   For  any  two  columns  of  Pj  and  Pj  of  P,  their  Lie  Bracket  [P;,  Pj]  is  a  linear 

combination  of  the  columns  of  P. 
The  Lie  bracket  of  two  vectors  Pj  and  Pj5  that  are  both  functions  of  e  is  the 
vector 

[P„  Pj]  =  (dF.Jde)?.,  -  (dPJde)?, 
where  dPjde  and  d?}/de  are  the  n  x  n  matrix  of  partial  derivatives. 

To  determine  whether  this  is  a  linear  combination  of  the  columns,  consider  the 
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extended  n  x  (m+ 1)  matrix  (P,  [P(,  PJ)  and  check  if  its  (m+ 1)  x  (m+ 1)  minors  are 

zero. 

THEOREM  2.3  [9,10]:  Let  D  be  an  m-dimensional  distribution.  Then  there  exists  an 

integral  surface  for  D,  containing  a  point  e,  if  and  only  if  D  is  involutive  in  an  open 

neighborhood  of  e. 

THEOREM  2.4:  Let  D  be  a  distribution  associated  with  a  local  control  strategy  P. 

Let  U  be  an  open  subset  of  W,  and  let  6p  e  *  be  a  point  such  that  f(9p)  e  U.  Then 

P  is  repeatable  at  9,,  over  U,  if  and  only  if  there  exists  an  integral  surface  S 

containing  9p,  such  that  the  restriction  of  f  to  S  is  a  homeomorphism  of  S  onto  U. 

THEOREM  2.5:  Let  D  be  a  distribution  associated  with  the  control  strategy  P,  and 

let  S  be  an  integral  surface  for  D,  over  a  simply  connected  underlying  set  U  c  W. 

Then  S  intersects  each  fiber  at  no  more  than  one  point. 

It  is  well  known  from  differential  geometry  (for  example,  [8])  that  if  S  c  Rn  is  any 
smooth  m-dimensional  surface,  and  u,  v  are  any  two  tangent  vectors  to  S  at  a  point 
e,  then  their  Lie  bracket  [u,  v]  is  also  a  tangent  vector  to  S  at  e. 

A  distribution  D  is  called  involutive  at  e  if,  for  any  two  vectors  u,  v  that  belong 
to  a  subspace  D§(u  and  v  are  both  evaluated  at  e),  their  Lie  bracket  [u,  v]  also 
belongs  to  the  subspace  De. 

From  theorem  2.3  and  theorem  2.4,  it  is  clear  why  repeatability  implies  the  Lie 
bracket  condition  (LBC)  stated  in  theorem  2.2.  If  our  control  P  is  repeatable,  then 
there  exists  an  integral  surface  for  the  distribution.  Such  a  surface  exists  if  and  only 
if  LBC  holds. 


20 
It  is  important  to  note  that  if  the  LBC  holds  at  every  point,  then  there  is  a 

foliation   of   integral    surfaces    and    the    control    is    repeatable    for    any   initial 

configuration.  If  integral  surfaces  do  exist  for  a  given  distribution,  then  they  must 

either  be  disjoint  or  coincide,  because  at  each  point,  the  distribution  assigns  a  unique 

tangent  space.  In  other  words,  if  the  distribution  is  involutive  at  every  point  e  e  *, 

then  we  obtain  a  family  of  disjoint  surfaces  (called  a  foliation)  which  covers  *. 

From  the  above  definition  2.1,  an  inverse  kinematic  equation  can  be  expressed 

: 

in  the  form 

<Se  =  P(e)  D  . 
For  example,  the  general  pseudo-inverse  is  given  by 

pg(e)  =  (MmrwtfJKMmrw}-1. 

the  weighted  pseudo-inverse  is  given  by 

pw(e)  =  [kyWajprtJ]1)-1, 
and  the  Moore-Penrose  pseudo-inverse  is  given  by 

Pmp(e)  =  [JFCPW)1. 

If  we  consider  one  degree  of  redundancy  (n-m  =  1),  the  Lie  bracket  condition 
(LBC)  can  be  expressed  in  the  form 

LBC=  |  P  [Ps,  Pj]  |  =0. 
Suppose  that  m  =  3  and  n  =  4,  then 

LBC=  iPfP^PJl   =  |P[P2,P3]|   =  I P  [P3,  Pxl I   =0. 

The  computations  involving  the  Lie  bracket  condition  are  very  difficult  to  do  by 
hand,  but  they  can  be  done  with  the  use  of  Maple,  Mathematica  or  another  symbolic 


software  package. 

Even  the  simplest  case  (a  planar  3R  manipulator)  requires  a  huge  amount  of 
memory  in  order  to  obtain  a  symbolic  expression  of  the  Lie  bracket  condition.  In  this 
dissertation  a  numerical  verification  for  repeatability  is  done  in  terms  of  the  Lie 
bracket  condition. 


CHAPTER  3 
REPEATABILITY  OF  PLANAR  SERIAL  REDUNDANT  MANIPULATORS 


The  proposed  general  pseudo-inverse  control  in  chapter  2  is  applied  to  planar 
manipulators  (3R,  PRR,  4R). 


3.1  Planar  3R  Manipulator 
Consider  a  3  degree  of  freedom  manipulator  which  moves  in  the  (x,y)  plane. 
When  only  the  position  of  the  end  effector  is  of  concern,  this  manipulator  is 
kinematically  redundant.  The  kinematic  model  of  a  planar  3R  manipulator  is  shown 
in  Fig.  3-1. 
3.1.1  Derivation  of  Inverse  Kinematic  Equation 

The  coordinates  for  a  point  P  (see  Fig.  3-1)  in  the  end  effector  are  given  as 

x  =  a^q  +  a23C1+2  +  a34C1+2+3,  (3-1) 

y    =    ai2Sl    "*"    a23Sl+2    "•"    a34Sl+2  +  3> 

where  an,  a23,  a^  represent  the  length  of  each  link,  while  the  variables  with  subscript 
are  defined  as 

ss  =  sinfo),       si+j+...+n  =  sin(ei+ej  +  ...  +  en), 

c,  =  cos(6i),      ci+j+...+n  =  cos(el+ej+...  +  eB). 
Eq.  (3.1)  for  an  instantaneous  motion  can  be  expressed  as 

22 
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D 


P:  Position  (x.  y)  of  end  effector 

a  :  Orientation 


X 


Fig.  3-1:  Kinematic  Model  of  a  Planar  3R  Manipulator 
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"6x 

"69! 

=    [J] 

692 

-*y    - 

.se3  . 

(3.2) 


The  Jacobian  matrix  is  obtained  as 


[J]  = 


"aUSl"a23Sl+2"a34Sl+2  +  3  "a23Sl+2"a34Sl+2  +  3  ~a34Sl+2  +  3 

ai2Cl  +  a23Cl+2"'"a34Cl+2  +  3  a23Cl  +  2+  a34Cl+2  +  3  a34Cl  +  2  +  3 


(3.3) 


The  joint  torques  acting  in  each  joint  combine  to  give  resultant  forces  at  the  end 
effector,  and  these  can  be  expressed  as 


3    j 


=     [J]T 


t 


fy 


Joint  torques  are  related  to  joint  rotations  by 


3   J 


=    [k] 


V 

"   ©01 

92- 

'  ©02 

A- 

"   ©03     - 

where  [k]  is  a  3x3  diagonal  matrix  of  joint  stiffnesses  as  follows 


[k]  = 


k,      0 

0       k, 


0 
0 


0       k 


3      J 


(3.4) 


(3.5) 


k;  is  the  nonzero  ith  joint  stiffness  that  has  dimensions  (FL)  and  e{  -  90i  is  the 
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difference  between  the  current  and  the  free  position  joint  angle,  i  =  1,  2,  3. 


Thus  eq.  (3.5)  can  be  written  in  the  form 


Gj-e 


01 


©2  "   ©02 


e3  -  e03 


=  M"1 


(3.6) 


Substituting  eq.  (3.4)  into  eq.  (3.6)  yields 


Gi-9 


01 


©2  "   ©02 


©3  "   ©03 


irnT 


=  WW 


fy 


(3.7) 


Premultiplying  the  left  and  right  sides  of  eq.  (3.7)  by  [J]  yields 


[J] 


Gj  -  G 


01 


©2   -   ©02 


©3   _   ©03 


irnT 


-  [Jprm 


% 


(3.8) 


Provided  that  [J]  is  of  full  rank,  eq.  (3.8)  can  be  expressed  in  the  form 


"4" 

©1  "  ©01 

A 

-  (  iww  y  [j] 

©2  "   ©02 
-     ©3  "   ©03 

(3.9) 


Taking  the  derivative  eq.  (3.7)  yields 


where 


26 


<SG, 


<se, 


<se 


3     J 


=  MW 


St 


L«U 


IrrTlT 


+  [k]1^!] 


(3.10) 


[6J]T  = 


<SJ„  SJ 


'ii 


'21 


'31 


12 


(SJ-),  <SJ 


22 


5Lt         <5J 


32 


(3.11) 


-    [u] 


set     0" 

+  M 

0       592_ 

<se2      0 


.0       se3 


+    [w] 


<563      0 


.0        6ex  . 


[u]  = 


Ull       uu 

U21  U^ 

u31     u32 


M 


N    = 


wu 

«U    ' 

W2i 

W^ 

W31 

W32     . 

«JU  =  -&aclS61  -  833^+2(661+562)  -  a34c1+2+3(6e1+5e2+6e3)) 


5Jn  =  -a^M^i  -  a^s^se^se^  -  ^s^^se^  se2+ se3), 
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*J21  =  -  aaC^sei+S©*)  -  a34c1+2+3(*e1+*e2+*e3)> 
6Jn  =  -  a^Ase^  se2)  -  a34s1+2+3(5e1+5e2+5e3), 
«sj31  =  -  a34c1+2+3(«e1+  se2+se3), 
6Jn  =  -  a^^^se^  se2+ se3), 

uu  =  -a^Cj  -  a23c1+2  -  a34c1+2+3,         uu  =  -  a23s1+2  -  a34s1+2+3, 

U21    =    "  a23Cl+2  "  a34Cl+2  +  3» 

U31    =    '  a34Cl+2  +  3> 

Vll    =    "  a23Cl  +  2  "  a34Cl  +  2  +  3> 

V21    =    '  a23Cl+2  "  a34Cl+2  +  3» 

V31    =    "  a34Cl+2  +  3> 

Wll    =    "  a34Cl  +  2  +  3> 

W21    =    "  a34Cl+2  +  3» 


U22  = 

-  a23s1+2  -  a^Si+2+3, 

U32  = 

"  a34Sl+2+3> 

Vn  = 

"  a34Sl+2+3» 

V22    = 

"  a34Sl+2+3> 

V32    —    "  a34Sl+2  +  3» 


W12    "~    "   ai2Sl  "  a23Sl  +  2  "  a34Sl+2  +  3» 


W22    —    "   a23Sl+2  "  a34Sl  +  2  +  3> 


W31 a34Cl+2  +  3> 


W32    ~    "  a34Sl+2  +  3- 


The  second  term  of  the  right-hand  side  of  eq.  (3.9)  goes  to  the  left-hand  side, 


se2 
<se7 


IfrnT 


-  [kn*j] 


% 


-irnT 


MV] 


si 


Sf, 


(3.12) 


Substituting  eq.  (3.9)  into  eq.  (3.12)  and  rearranging  the  left-hand  side  of  eq.(3.12) 


yields 
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where 


[T] 


59j 

se2 
.«e3 


lrnT 


=  WV] 


St 


*fy 


(3.13) 


[T]  = 


Tu 

Tu 

T21 

T* 

T« 

T32 

13 


l23 


■33 


Tn  =  1  -  (  uufx  +  w^  )/k1,  Tn  =  -( vufx  +  u^  )/k1, 

Tn  =  -(  wnfx  +  vjy  )/klt  T21  =  -(  u21fx  +  w^  )/k2, 

T22  =  1  -  (  v21fx  +  u^fy  )/k2,  T23  =  -(  w21fx  +  V22f;  )/k2, 

T31  =  -(  u31fx  +  w32£,  )/k3,  T32  =  -(  v31fx  +  u32t;  )/k3, 

T33  =  1  -  (  w31fx  +  v32fy  )/k3. 
Provided  that  [T]  is  of  full  rank,  eq.  (3.13)  can  be  expressed  in  the  form 


<SG2 
59, 


irnT 


=    [A]X[J] 


«4 


(3.14) 


Letting 


[A]  =  [k][T], 


and  premultiplying  the  left  and  right  sides  of  eq.  (3.14)  by  [J]  yields 
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[J] 


69, 


69, 


66, 


-irnT 


=  [JHAHJ] 


Si, 


(3.15) 


Substituting  eq.  (3.2)  into  eq.  (3.15)  yields 


~6x  ' 

=  [C] 

*«t 

-*y  - 

.«*    - 

(3.16) 


where 


irnT 


[C]  =  [JllAJ^J] 


Provided  [T]  and  [k]  are  both  of  full  rank,  eq.  (3.16)  can  be  expressed  in  the  form 


X" 

=  [C]1 

~6x  ' 

A- 

sy  . 

(3.17) 


Substituting  eq.  (3.17)  into  eq.  (3.14)  yields  a  general  pseudo-inverse  equation 


69, 


69, 


69, 


[A]-W(  [mwv  r1 


Sx 


L6y 


(3.18) 


irnTrrxi-l 


=  [Ai-WIPl 


Sx 


&y    J 
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3.1.2  Numerical  Verification  and  Simulation  of  Repeatability 

For  the  first  example,  the  system  consists  of  three  revolute  joints  as  shown  in  Fig. 
3-1  and  link  lengths  are  chosen  as  a^  =  1.0,  a^  =  1.0,  and  a^  =  1.0  (m).  The  ranges 
of  each  joint  limit  are  assumed  as  follows: 

-170°  <  Gx  <  180° 

-180°  <  e2  <  170° 

-175°  <  e3  <  175° 
Thus  the  stiffnesses  for  each  joint  are  taken  as  being  equal  (kx  =  k2  =  k3  =  1.0 
Nm/radian)  based  on  these  joint  limits.  The  free  position  angles  of  each  joint  are 
chosen  as  e01  -  5°,  e02  =  -5°,  G03  =  0°.  The  x-y  coordinates  of  the  four  vertices  of 
the  command  path  (1000  consecutive  equidistant  segments  each  side)  in  the  task 
space  are  successively  given  counterclockwise  from  the  lower  left  vertex  as  follows: 

(0.5,  0.6)        (0.6,  0.6) 

(0.5,  0.5)        (0.6,  0.5) 
where  the  units  are  meters.  The  end  effector  is  to  move  along  a  predefined  square 
path  with  a  constant  speed  of  10  mm/sec  and  a  time  step  size  At  =  0.01  sec. 

Section  2.4  demonstrated  theoretically  the  requirement  of  repeatability  for  the 
kinematic  control  strategy  of  the  serial  redundant  manipulator.  Succinctly,  the  joint 
angles  repeat  for  a  cyclic  motion  of  end  effector  only  when  the  Lie  bracket  condition 
holds  for  the  kinematic  control  strategy.  Section  2.4  also  discussed  how  this 
repeatability  is  independent  of  the  initial  configuration  (choice  of  initial  joint  angles). 
These  concepts  will  now  be  validated  numerically  for  the  planar  3R  manipulator. 
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Different  initial  configurations  corresponding  to  the  same  end  effector  position 

(x  =  0.5,  y  =  0.5)  can  be  obtained  by  assigning  the  end  effector  orientation  a  (see 

Fig.  3-1).  In  the  reverse  analysis,  two  sets  of  solutions  can  be  obtained  for  the  given 

position  and  orientation  of  the  end  effector  (see  Duffy  [12]). 

The  numerical  verification  of  the  LBC  is  demonstrated  by  the  orders  of 
magnitude  difference  between  the  LBC  columns  in  Table  3-1  and  3-2  (twenty  degree 
increments  for  a  are  displayed.  But  the  analysis  was  performed  in  one  degree 
increment.).  Table  3-1  shows  that  Lie  bracket  condition  values  are  near  zeroes  (10"° 
-  10"8)  for  different  initial  configurations  when  the  general  pseudo-inverse  equation 
is  used.  On  the  other  hand  Table  3-2  shows  that  numerical  Lie  bracket  condition 
values  are  not  zeroes  for  different  initial  configurations  when  the  Moore-Penrose 
pseudo-inverse  equation  is  used. 

Three  different  initial  configurations  (see  Fig.  3-2)  for  the  same  end  effector 
position  are  chosen  from  Table  3-1.  For  example,  first  initial  configuration  at 
maximum  potential  energy  (e1  =  -155.7°,  e2  =  -138.59°,  e3  =  -65.7°),  second  initial 
configuration  at  intermediate  potential  energy  (ex  =  -129.06°,  e2  =  146.01°,  e3  = 
63.04°)  and  third  initial  configuration  at  minimum  potential  energy  (9j  =  -37.33°, 
e2  =  87.19°,  e3  =  110.13°)  are  chosen.  For  these  three  different  initial 
configurations,  Lie  bracket  condition  values  during  the  10  cycle  motion  are  shown  in 
Figs  3-3,  3-4,  and  3-5.  The  initial  and  the  final  joint  angles  through  10  cycles  of 
motion  of  the  end  effector  are  generated  in  Table  3-3.  At  the  end  of  the  10  cycles 
the  position  difference  (xfmal  -  x^,,  yfinal  -  yinitial)  and  the  joint  angle  difference  ( 
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Table  3-1 

General  Pseudo-Inverse  Equation 

a          LBC 

X 

Y 

TH1 

TH2 

TH3 

PE 

0  2.391285e-07 

0.5000 

0.5000 

-155.7048 

-138.5904 

-65.7048 

8.670344 

0  8.037865e-07 

0.5000 

0.5000 

65.7048 

138.5904 

155.7048 

6.307208 

20  2.822548e-09 

0.5000 

0.5000 

-123.2720 

-152.9815 

-63.7465 

7.881616 

20  1.134500e-06 

0.5000 

0.5000 

83.7465 

152.9815 

143.2720 

6.648197 

40  -1.699821e-07 

0.5000 

0.5000 

-70.4574 

-162.6340 

-86.9086 

7.216641 

40  1.845255e-08 

0.5000 

0.5000 

126.9086 

162.6340 

110.4574 

7.038628 

60  2.862487e-07 

0.5000 

0.5000 

-10.5431 

-158.9085 

-130.5483 

7.535189 

60  -3.383497e-07 

0.5000 

0.5000 

-169.4516 

158.9085 

70.5432 

8.601602 

80  3.885419e-07 

0.5000 

0.5000 

16.9563 

-146.0181 

-150.9381 

7.675252 

80  2.043804e-07 

0.5000 

0.5000 

-129.0618 

146.0181 

63.0437 

5.969477 

100  2.257927e-07 

0.5000 

0.5000 

29.7393 

-130.9622 

-158.7770 

7.370122 

100  2.339417e-07 

0.5000 

0.5000 

-101.2229 

130.9622 

70.2608 

4.511340 

120  -5.409503e-08 

0.5000 

0.5000 

37.7244 

-115.6575 

-162.0668 

6.920488 

120  1.207760e-07 

0.5000 

0.5000 

-77.9331 

115.6575 

82.2757 

3.602386 

140  -2.251315e-07 

0.5000 

0.5000 

43.9929 

-100.8564 

-163.1364 

6.454121 

140  6.572133e-08 

0.5000 

0.5000 

-56.8635 

100.8564 

96.0072 

3.080577 

160  -4.203282e-07 

0.5000 

0.5000 

49.8612 

-87.1995 

-162.6616 

6.022204 

160  4.425800e-08 

0.5000 

0.5000 

-37.3383 

87.1995 

110.1389 

2.876694 

180  -5.961847e-07 

0.5000 

0.5000 

56.1950 

-75.5215 

-160.6734 

5.646737 

180  4.398267e-08 

0.5000 

0.5000 

-19.3265 

75.5215 

123.8051 

2.935046 

200  2.795830e-06 

0.5000 

0.5000 

63.8168 

-66.9916 

-156.8251 

5.339623 

200  3.656248e-08 

0.5000 

0.5000 

-3.1748 

66.9916 

136.1833 

3.187467 

220  1.06906  le-05 

0.5000 

0.5000 

73.5563 

-62.9725 

-150.5837 

5.117468 

220  -1.577192e-07 

0.5000 

0.5000 

10.5839 

62.9725 

146.4438 

3.547688 

240  3.336771e-07 

0.5000 

0.5000 

85.9640 

-64.3415 

-141.6224 

5.017191 

240  1.612510e-06 

0.5000 

0.5000 

21.6226 

64.3415 

154.0361 

3.933885 

260  -3.237957e-08 

0.5000 

0.5000 

100.9853 

-70.7789 

-130.2062 

5.098199 

260  5.514746e-06 

0.5000 

0.5000 

30.2064 

70.7789 

159.0148 

4.306150 

280  9.248 140e-09 

0.5000 

0.5000 

118.1286 

-81.0507 

-117.0777 

5.423747 

280  5.093987e-08 

0.5000 

0.5000 

37.0779 

81.0507 

161.8716 

4.670212 

300  3.424381e-08 

0.5000 

0.5000 

136.9206 

-93.8423 

-103.0781 

6.045742 

300  -5.399946e-07 

0.5000 

0.5000 

43.0783 

93.8423 

163.0796 

5.047785 

320  6.938001e-08 

0.5000 

0.5000 

157.1842 

-108.1592 

-89.0249 

7.008238 

320  -4.092887e-07 

0.5000 

0.5000 

49.0250 

108.1592 

162.8160 

5.453265 

340  1.354084e-07 

0.5000 

0.5000 

179.2173 

-123.2896 

-75.9276 

8.366526 

340  -1.913770e-07 

0.5000 

0.5000 

55.9277 

123.2896 

160.7829 

5.883567 

360  2.414215e-07 

0.5000 

0.5000 

-155.7032 

-138.5932 

-65.7034 

8.670371 

360  8.035577e-07 

0.5000 

0.5000 

65.7036 

138.5932 

155.7034 

6.307222 
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Table  3-2 


Moore-Penrose  Pseudo-Inverse  Equation 


a  LBC 

0    -1.664743e+00 
0     3.449764e  +  00 
20  -2.359110e  +  00 
20    1.748641e  +  00 
40  -8.067412e-01 
40    6.070027e-01 
60  -9.0228 16e-01 
60    1.580794e  +  00 
80  -2.840095e  +  00 
80    2.233481e  +  00 
100 -3.464 188e  + 00 
100   1.188109e  +  00 
120-3.064482e  +  00 
120  7.010876e-01 
140-3.166571e  +  00 
140  5.498406e-01 
160-4.294162e  +  00 
160  5.814914e-01 
180-7.471548e  +  00 
180  8.518371e-01 
200-1.197545e  +  01 
200   1.796072e  +  00 
220-8.342236e  +  00 
220  5.09585  le  +  00 
240-2.963700e  +  00 
240   1.142484e  +  01 
260-1.180420e  +  00 
260  9.990296e  +  00 
280  -6.738749e-01 
280  5.537862e  +  00 
300  -5.441282e-01 
300  3.557440e  +  00 
320  -5.985302e-01 
320  3.021057e  +  00 
340  -8.8303 14e-01 
340  3.247780e  +  00 
360  -1.664950e  +  00 
360  3.449667e  +  00 


X 

0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 


Y 

0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 
0.5000 


TH1 

-155.7048 

65.7048 
-123.2720 

83.7465 

-70.4574 

126.9086 

-10.5431 

-169.4516 

16.9563 
-129.0618 

29.7393 
-101.2229 

37.7244 
-77.9331 

43.9929 
-56.8635 

49.8612 
-37.3383 

56.1950 
-19.3265 

63.8168 

-3.1748 

73.5563 

10.5839 

85.9640 

21.6226 
100.9853 

30.2064 
118.1286 

37.0779 
136.9206 

43.0783 
157.1842 

49.0250 
179.2173 

55.9277 
-155.7032 

65.7036 


TH2 

-138.5904 
138.5904 

-152.9815 
152.9815 

-162.6340 
162.6340 

-158.9085 
158.9085 

-146.0181 
146.0181 

-130.9622 
130.9622 

-115.6575 
115.6575 

-100.8564 

100.8564 

-87.1995 

87.1995 

-75.5215 

75.5215 

-66.9916 

66.9916 

-62.9725 

62.9725 

-64.3415 

64.3415 

-70.7789 

70.7789 

-81.0507 

81.0507 

-93.8423 

93.8423 

-108.1592 
108.1592 

-123.2896 
123.2896 

-138.5932 
138.5932 


TH3 

-65.7048 

155.7048 

-63.7465 

143.2720 

-86.9086 

110.4574 

-130.5483 

70.5432 

-150.9381 

63.0437 

-158.7770 

70.2608 

-162.0668 

82.2757 

-163.1364 

96.0072 

-162.6616 

110.1389 

-160.6734 

123.8051 

-156.8251 

136.1833 

-150.5837 

146.4438 

-141.6224 

154.0361 

-130.2062 

159.0148 

-117.0777 

161.8716 

-103.0781 

163.0796 

-89.0249 

162.8160 

-75.9276 

160.7829 

-65.7034 

155.7034 
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Y 


Hqx,  pe 


@   -155.7,  -138.5,  -65.7  (deg) 
@   -129.0,  146,0,  63,0 

Q)  -37,3;  87,1,  110.1 


Internediate  pe 


Fig.  3-2:  Three  Different  Initial  Configurations  of  Planar  3R  Manipulator 
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LBC    VS    CYCLIC    MOTION 

INITIAL   JOINT    ANGLE:     -155    7,     -138.5,     -65.6 


2  4  6  8  10 

CYCLECSYU80L:    GENERAL    PSEUOO- I NVER5E,    SOLID     M0OP.E-PENRO5E    PSEUOO-  I NVEBSE5 


Fig.  3-3:  Lie  Bracket  Condition  Values  Through  10  Cycles  of  the  Motion 
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LBC    VS    CYCLIC    MOTION 

Initial  Joint  Angle:  -129,  146,  63 


o 

CD 


0  inTTTTTTTTTTrTTTTTTmTTrTTrTTT^^ 


0  2  4  6  8  10 

CycleC  SyTOoi :   General   Pseudo inverse,    Sol  la     Moore-Penrose  Pseuao inverse) 


Fig.  3-4:  Lie  Bracket  Condition  Values  Through  10  Cycles  of  the  Motion 
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LBC    VS    CYCLIC    MOTION 


Initial  Joint  Angle:  -37.3,  87.1,  110.1 


o 

CD     0.4  - 


o  i  ;.xnco:Km:^m:t>:>>>>>:*>>>>:HC»>:f.4>^^ 


0  2  -t  6  8  10  12 

CycleC  Symbol:   General   Pseuaol nverse.     Solid     Moore-Penrose  Pseudol nverse3 


Tig.  3-5:  Lie  Bracket  Condition  Values  Through  10  Cycles  of  the  Motion 
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(ei)fmai  -  (ei)initiai  >  i  =  1,  2,  3)  are  also  shown  in  Table  3-3.  The  simulation  results 
given  in  Figs  3-3,  3-4,  3-5,  and  Table  3-3  are  a  clear  comparison  of  the  general 
pseudo-inverse  control  and  the  Moore-Penrose  pseudo-inverse  control.  By  the  orders 
of  magnitude  comparison,  it  is  clear  that  the  general  pseudo-inverse  control  (eq.  3.18) 
is  repeatable  in  practice  whereas  the  Moore-Penrose  pseudo-inverse  control  (eq. 
2.48)  is  not. 

An  additional  explanation  for  repeatability  can  be  done  by  considering  potential 

n 

energy  curves  (pe  =  2  0.5ki(©,  -  G0i)2)  for  different  initial  configurations.  If  these 
curves  are  never  intersecting  each  other  during  the  cyclic  motion,  then  pe  is  cyclic 
and  the  initial  joint  angles  return  to  their  initial  values  when  the  end  effector  returns 
to  its  initial  position.  Fig.  3-6  shows  the  overall  potential  energy  curves  from 
maximum  pe  curve  to  low  pe  curve.  Fig.  3-7  shows  curves  of  near  intermediate  pe 
curve  of  Fig.  3-6.  From  Figs  3-6  and  3-7,  the  potential  energy  curves  are  not 
intersecting  each  other  during  the  cyclic  motion  when  the  general  pseudo-inverse 
equation  is  used. 

As  stated  in  section  2.4,  if  the  Lie  bracket  condition  holds  at  every  point  e  e  *, 
then  we  obtain  a  family  of  disjoint  surfaces  (called  a  foliation)  which  covers  $.  The 
potential  energy  curves  in  Fig.  3-6  and  3-7  are  a  family  of  potential  energy  curves 
which  are  never  intersecting  each  other. 

In  the  second  example,  the  link  lengths  are  chosen  as  an  =  0.3,  a^  =  0.425,  a^ 
=  0.1  (m),  the  general  pseudo-inverse  equation  is  also  used.  The  ranges  of  each  joint 
limit  are  also  assumed  as  follows: 


Table  3-3 
Simulation  Results  Through  10  Cycles  of  the  Motion 
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General  Pseudo-Inverse  Equation 


CYCLE 

LBC 

X 

Y 

TH1 

TH2 

TH3 

0 

2.513085e-07 

0.5000 

0.5000 

-155.7048 

-138.5904 

-65.7048 

10 

2.502815e-07 

0.5009 

0.5008 

-155.7293 

-138.6359 

-65.5864 

DIFF. 

0.0009 

0.0008 

-0.0245 

-0.0455 

0.1184 

0 

1.983124e-07 

0.5000 

0.5000 

-129.0618 

146.0181 

63.0437 

10 

2.0809 15e-07 

0.5008 

0.5009 

-129.0571 

146.0755 

62.9211 

DIFF. 

0.0008 

0.0009 

0.0047 

0.0574 

-0.1226 

0 

4.482646e-08 

0.5000 

0.5000 

-37.3383 

87.1995 

110.1389 

10 

4.406034e-08 

0.5010 

0.5008 

-37.3323 

87.1560 

110.1093 

DIFF. 

0.0010 

0.0008 

0.0060 

-0.0435 

-0.0296 

Moore-Penrose  Pseudo-Inverse  Equation 


CYCLE 

LBC 

X 

Y 

TH1 

TH2 

TH3 

0 

-1.664750e  +  00 

0.5000 

0.5000 

-155.7048 

-138.5904 

-65.7048 

10 

-2.043967e  +  00 

0.5009 

0.5008 

-146.5125 

-143.4404 

-63.5879 

DIFF. 

0.0009 

0.0008 

9.1923 

-4.8500 

2.2169 

0 

2.233483e  +  00 

0.5000 

0.5000 

-129.0618 

146.0181 

63.0437 

10 

1.848593e  +  00 

0.5008 

0.5009 

-118.6704 

140.9852 

64.5052 

DIFF. 

0.0008 

0.0009 

10.3914 

-5.0329 

1.4615 

0 

5.815293e-01 

0.5000 

0.5000 

-37.3383 

87.1995 

110.1389 

10 

5.977754e-01 

0.5010 

0.5008 

-34.7540 

85.4007 

112.0434 

DIFF. 

0.0010 

0.0008 

2.5843 

-1.7988 

1.9045 
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P.     E.     VS    CYCLIC    MOTION 

2 

9 

e 

7 

UJ 

6 

a 

5 
4 
3 

f — "X^— -See  Fig.    3-7 

■    \J 

C 

O.Z                                 0   4                                 0.6                                 0   9                                    1                                    1 

SQUARE    PATH   C    0   -    0    25    -     0.5-    0.75-     1} 
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-80°  <  6,  <  90° 

-160°  <  e2  <  180° 

-160°  <  e3  <  180° 

Thus  the  free  position  angles  of  each  joint  can  be  also  chosen  as  e01  =  5°,  e02  =  e03 

=  10°).  The  initial  joint-angles  are  9  =  [40°,  40°,  70°].  The  end  effector  is  to  track 

a  straight  line  from  point  (0.21701,  0.66138)  to  point(-0.34299,  0.66138)  with  a 

constant  speed  of  20  mm/sec.  Fig.  3-8  shows  the  simulation  result  when  the  joint 

stiffnesses  are  given  values  such  as  lq  =  k2  =  k3  =  1  (Nm/radian).  The  first  joint 

angle  (6j  =  90.99°)  is  not  within  the  range  of  joint  limit  while  the  second  (e2  = 

33.1°)and  the  third  joint  angle  (e3   =   50.52°)  are  within.  Fig.  3-9  shows  the 

simulation  result  when  the  joint  stiffnesses  are  given  values  such  that  the  first  joint 

is  stiffer  than  the  other  two  joints  (k,  =  10,  k2  =  1,  k3  =  1).  All  the  joint  angles  (ex 

=  85.22°,  G2  =  45.24°,  G3  =  26.55°)  are  within  the  range  of  joint  limits  during  the 

specified  path.  These  two  figures  (Fig.  3-8  and  Fig.  3-9)  indicate  that  stiffer  joint 

moves  less  than  softer  joint.  Thus  a  proper  choice  of  stiffness,  k;,  based  on  the 

mechanical  limits  of  joint  helps  to  avoid  the  joint  limit  during  the  specified  path. 

3.2  Planar  PRR  Manipulator 

3.2.1  Derivation  of  Inverse  Kinematic  Equation 

The  kinematic  model  of  a  planar  PRR  manipulator  is  shown  in  Fig.  3-10. 

The  coordinates  for  a  point  P  (see  Fig.  3-10)  in  the  end  effector  are  given  as 
x  =  ssj  +  a23c2  +  aj^.,  (3.19) 

y    =    a23S2    "*"    a34S2  +  3 
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Fig.  3-8:  Simulation  Result  with  kx  =  k2  =  k3  =  1.0  (Nm/radian) 


Fig.  3-9:  Simulation  Result  with  kj  =  10,  k2  -  k3  =  1.0  (Nm/radian) 
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Fig.  3-10:  Kinematic  Model  of  a  Planar  PRR  Manipulator 
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where  a23,  a^  represent  the  length  of  each  link  and  sSj  is  defined  as  a  displacement 

of  the  prismatic  joint. 

The  Jacobian  matrix  is  obtained  as 


[J]  = 


1.0  "a23S2"a34S2  +  3  "a34S2  +  3 


0.0  a23C2  +  a34C2  +  3  a34C2  +  3 


The  general  pseudo-inverse  equation  is  obtained  as 


(3.20) 


6ssx 
<se, 


<se 


3    J 


ptwwc  mm-ww ) 


6x 


<5y 


(3.21) 


Detailed  derivation  of  the  inverse  kinematic  equation  for  a  planar  PRR  redundant 

serial  manipulator  is  presented  in  Appendix  A. 

3.2.2  Numerical  Verification  and  Simulation  of  Repeatability 

For  the  first  example,  the  system  consists  of  one  prismatic  and  two  revolute  joints 
as  shown  Fig.  3-10.  Link  lengths  are  chosen  as  a23  =  a^  =  1  (m).  The  ranges  of  each 
joint  limit  are  assumed  as  follows: 

-1  (m)  <  ss,  <  2  (m) 

-180°  <  e2  <  160° 

-180°  <  e3  <  160°. 
The  stiffnesses  for  each  joint  are  taken  as  kt  =  1  (N/m),  and  k2  =  k3  =  1.0 
(Nm/radian).  The  free  position  angles  of  each  joint  can  be  chosen  ss01  =  0.5  (m),  and 
eo2  =  eo3  =  -10°.    The  initial  values  are  sSj  =  1.0  (m),  e2  =  30°,  and  e3  =  -150°. 
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The  x-y  coordinates  of  the  four  vertices  of  the  command  path  (1000  consecutive 
equidistant  segments  each  side)  in  the  task  space  are  successively  given 
counterclockwise  from  the  lower  left  vertex  as  follows: 

(1.3660,  -0.2660)  (1.4660,  -0.2660) 

(1.3660,  -0.3660)  (1.4660,  -0.3660) 

where  the  units  are  meters.  The  end  effector  is  to  move  along  the  square  path  with 
a  constant  speed  of  10  mm/sec  and  a  time  step  size  At  =  0.01  sec. 

Lie  bracket  condition  values  through  10  cycles  of  the  motion  are  shown  in  Fig. 
3-11.  The  initial  and  the  final  joint  angles  through  each  cyclic  motion  of  the  end 
effector  are  generated  in  Table  3-4.  At  the  end  of  the  10  cycles  the  position 
difference  (  xfinal  -  xinitia„  yfinal  -  y^J  and  the  joint  angle  difference  (  (ss^  -  {md*m* 
(©i)nnai  -  (©initial  >  i  =  2,  3)  are  also  shown  in  Table  3-4.  By  the  orders  of  magnitude 
comparison,  it  is  clear  that  the  general  pseudo-inverse  control  (eq.  3.21)  is  repeatable 
in  practice  whereas  the  Moore-Penrose  pseudo-inverse  control  (eq.  2.48)  is  not. 

In  the  second  example,  the  same  link  lengths,  initial  joint  angles  and  joint 
stiffnesses  as  the  first  example  are  used.  This  example  is  to  illustrate  that  the  results 
are  independent  of  choice  of  unit  of  length.  The  desired  end  effector  twist  is  D  = 
(  0.0001  m,  0.0  m  )T.  The  resulting  joint  motions  are  obtained  via  an  application  of 
the  general  pseudo-inverse  equation: 

<se    =  (  Ssst  m,  se2  deg,  <se3  deg  )T 

=  (  0.000119  m,  -0.001092  deg,  -0.000799  deg  )T. 
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LBC    VS    CYCLIC    MOTION 


LBC:  LIE  BRACKET  CONDITION 
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Fig.  3-11:  Lie  Bracket  Condition  Values  Through  10  Cycles  of  the  Motion 


Table  3-4 


Simulation  Results  Through  10  Cycles  of  the  Motion 
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General  Pseudo-Inverse  Equation 


CYCLE 

LBC 

X 

Y 

SSi 

TH2 

TH3 

0.000 

-0.0000038 

1.366025 

-0.366025 

1.000000 

30.000013 

-150000066 

1.000 

-0.000004 

1.366105 

-0.365935 

1.000105 

29.997632 

-150000682 

2.000 

-0.000004 

1.366186 

-0.365844 

1.000234 

30.003800 

-150006499 

3.000 

-0.000004 

1.366267 

-0.365754 

1.000364 

30.009970 

-150012316 

4.000 

-0.000004 

1.366347 

-0.365664 

1.000493 

30.016142 

-15Q018132 

5.000 

-0.000004 

1.366428 

-0.365574 

1.000622 

30.022316 

-150.023948 

6.000 

-0.000004 

1.366509 

-0.365484 

1.000751 

30.028491 

-150.029763 

7.000 

-0.000004 

1.366589 

-0.365394 

1.000880 

30.034669 

-150.035577 

8.000 

-0.000004 

1.366670 

-0.365303 

1.001010 

30.040849 

-150.041391 

9.000 

-0.000004 

1.366751 

-0.365213 

1.001139 

30.047031 

-150047204 

10.000 

-0.000004 

1.366831 

-0.365123 

1.001268 

30.053214 

-150.053016 

DIFF. 

0.000806 

0.000902 

0.001268 

0.053201 

-0.05235 

Moore-Penrose  Pseudo-Inverse  Equation 


:ycle 

LBC 

X 

Y 

ss, 

TH2 

TH3 

0.000 

2.610043 

1.366025 

-0.366025 

1.000000 

30.000013 

-150000066 

1.000 

2.610684 

1.366110 

-0.365932 

0.987656 

30.692766 

-149.473779 

2.000 

2.600207 

1.366194 

-0.365839 

0.975260 

31.361522 

-148.940235 

3.000 

2.578829 

1.366278 

-0.365747 

0.962832 

31.996296 

-148396360 

4.000 

2.547220 

1.366361 

-0.365654 

0.950435 

32.595150 

-147545347 

5.000 

2.506420 

1.366444 

-0.365561 

0.938130 

33.157023 

-147290315 

6.000 

2.457731 

1.366527 

-0.365468 

0.925970 

33.681691 

-146.734159 

7.000 

2.402590 

1.366610 

-0.365374 

0.913997 

34.169666 

-146.179456 

8.000 

2.342458 

1.366692 

-0.365280 

0.902246 

34.622052 

-145.628402 

9.000 

2.278733 

1.366775 

-0.365185 

0.890743 

35.040392 

-145.082806 

10.000 

2.212683 

1.366858 

-0.365090 

0.879506 

35.426522 

-144544104 

DIFF. 

0.000833 

0.000935 

-0.120494 

5.426509 

5.45596 
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These  same  results  are  obtained  if  the  unit  of  length  measure  for  all  elements 

is  changed  to  centimeters.  In  this  case,  the  stiffnesses  are  kx  =0.01  (N/cm),  and  k2 

=  k3  =  100  (Ncm/radian).  The  results  can  be  recalculated  and  shown  to  be  identical 

to  the  above,  bearing  in  mind  the  changed  dimension  for  the  incremental  length 

change  of  the  prismatic  joint,  viz. 

69    =  (  Sss1  cm,  se2  deg,  se3  deg  )T 

=  (  0.0119  cm,  -0.001092  deg,  -0.000799  deg  )T. 

This  clearly  illustrates  that  the  analysis  is  independent  of  the  chosen  unit  of 
length.  This  is  not  the  case  when  the  Moore-Penrose  pseudo-inverse  equation  is  used, 
which  is  analogous  to  the  minimum-norm  solution.  In  such  a  case,  there  is  no  means 
available  in  the  analysis  whereby  the  units  of  the  physical  dimensions  or  of  the 
mechanical  properties  can  be  accounted  for.  Here,  this  compensation  takes  place  by 
virtue  of  the  elastic  relationships  that  are  modeled  from  the  real  manipulator  system 
or  are  specified  by  virtue  of  n  desired-joint-stiffnesses. 

3.3  Planar  4R  Manipulator 

Consider  a  4  degree  of  freedom  manipulator  which  moves  in  the  (x,y)  plane.  The 
position  and  the  orientation  of  the  end  effector  is  of  concern,  thus  making  it 
kinematically  redundant.  The  kinematic  model  of  a  planar  4R  manipulator  is  shown 
in  Fig.  3-12. 
3.3.1  Derivation  of  Inverse  Kinematic  Equation 

The  coordinates  for  a  point  P  (see  Fig.  3-12)  in  the  end  effector  are 

x  =  a^Cj  +  a23C1+2  +  a34C1+2+3  +  <*45c1+2+3+4,  (.3.19) 
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Fig.  3-12:  Kinematic  Model  of  a  Planar  4R  Manipulator 
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y  _  aL2Sl  +  a23Sl+2  +  a34Sl+2  +  3  +  a45Sl+2+3+4> 


a  =  ex  +  e2  +  e3  +  e4. 
where  zn,  a^,  a^,  a^  represent  the  length  of  each  link  and  a  represents  the 
orientation  of  the  end  effector.  By  analogy  with  the  section  3.1.1,  the  Jacobian  matrix 
is  obtained  as 


[J]  = 


Jn        •'n       Ji3        Ji4 
J21        }n        Jjj        J24 

1.0      1.0      1.0       1.0 


where 


•Ml  =    "ai2Sl  "  a23Sl+2  "  a34Sl  +  2  +  3  "  a45Sl+2  +  3  +  4» 

Jl2  =    "a23Sl+2  "  a34Sl+2  +  3  "  a45Sl+2  +  3+4> 

•M3  =    "a34Sl+2+3  "  a45Sl+2+3+4> 

J 14  =    "a45Sl+2+3+4> 

J2I  =    ai2Cl    +    a23Cl  +  2    +    a34Cl+2  +  3    +    a45Cl+2  +  3  +  4> 

•'22  =    a23Cl+2    ■*■    a34Cl+2  +  3    "*"    a45Cl+2+3+4'J23    =    a34Cl+2+3    "*"    a45Cl+2+3+4> 

J24  =    a45Cl+2+3+4- 

The  general  pseudo-inverse  equation  is  obtained  as 


(3.20) 


5e2 

56, 


56 


4  J 


lrtiT  \-l 


=    [A]-W(  [J][A]'1[J]T  ) 


6x 
6y 
5a 


(3.21) 


Detailed  derivation  of  the  inverse  kinematic  equation  for  a  planar  4R  serial 
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redundant  manipulator  is  presented  in  Appendix  B. 

3.3.2  Numerical  Verification  and  Simulation  of  Repeatability 
Link  lengths  are  given  as 

au  =  0.7,  a^  =  0.9, 

a^  =  0.145,  a45  =  0.1  (m). 

The  ranges  of  each  joint  limit  are  assumed  as  follows: 

47°  <  e,  <  129°,  -120°  <  e2  <  -50°, 

-118°  <  e3  <  60°,  -180°  <  e4  <  180°. 

Based  on  above  the  range  of  each  joint  limitation  the  free  position  angles  of  each 
joint  can  be  chosen  as 

e01  =  (129  +  47)/2  =  88° ,  e02  =  (-120  -  50)/2  =  -85° , 

e03  =  (-118  +  60)/2  =  -29° ,         eM  =  (-180  +  180)/2  =  0°, 
and  the  stiffness  for  each  joint  are  also  taken  as 

kx  =  360/(129  -  47)  =  4.4,  k2  =  360/(-50  +  120)  =  5.14, 

k3  =  360/(118  +  60)  =  2.02,         k4  =  360/(180  +  180)  =  1.0  Nm/radian. 
The  initial  joint-angle  values  are  e  =  [100°,  -110°,  30°,  30°]T.  The  x-y  coordinates 
of  the  four  vertices  of  the  command  path  (2000  consecutive  equidistant  segments 
each  side)  in  the  task  space  are  successively  given  counterclockwise  from  the  lower 
left  vertex  as  follows: 

(0.9653,  0.9592)        (1.2653,  0.9592) 

(0.9653,  0.6592)        (1.2653,  0.6592) 
where  the  units  are  meters.  The  end  effector  is  to  track  the  square  path  with  a 
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constant  speed  of  15  mm/sec,  a  constant  orientation  a  =  50°,  and  a  time  step  size 

At  -  0.01  sec. 

Lie  bracket  condition  values  through  10  cycles  of  the  motion  are  shown  in  Fig. 
3-13.  The  initial  and  the  final  joint  angles  through  each  cyclic  motion  of  the  end 
effector  are  generated  in  Table  3-5.  At  the  end  of  the  10  cyclic  motions  of  the  end 
effector,  the  position  difference  (  x^  -  xinitial,  y^,  -  y^-J  and  the  joint  angle 
difference  (  (e^,  -  (e^)mmi  ,  i  =  1,  2,  3,  4)  are  also  shown  in  Table  3-5.  By  the 
orders  of  magnitude  comparison,  it  can  be  said  that  the  general  pseudo-inverse 
control  (eq.  3.21)  is  repeatable  in  practice  whereas  the  weighted  pseudo-inverse 
control  (eq.  2.46)  is  not. 
3.3.3  Implementation 

Actual  implementation  is  accomplished  on  a  planar  GE  4R  manipulator  (see  Fig. 
3-14).  The  same  link  lengths,  initial  joint  angles,  free  angles,  spring  stiffnesses,  and 
the  square  path  motion  of  the  end  effector  as  section  3.3.2  are  used. 

The  final  manipulator  configurations  after  10  cycles  of  the  motion  are  shown  in 
Figs.  3-15  and  3-16,  respectively.  From  the  Figs  3-15  and  3-16,  it  is  clear  that  the 
general  pseudo-inverse  control  is  repeatable  whereas  the  weighted  pseudo-inverse 
control  is  not. 
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LBC    VS    CYCLIC    MOTION 


LBC:  LIE  BRACKET  CONDITION 


u 

a. 
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CYCLECSYMBOL:  GENERAL  PSEUDO- I NVERSE,  SOLID:  WEIGHTED  PSEUDO- I NVERSE} 


Fig.  3-13:  Lie  Bracket  Condition  Values  Through  10  Cycles  of  the  Motion 


Table  3-5 


Simulation  Results  Through  10  Cycles  of  the  Motion 
General  Pseudo-Inverse  Equation 
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CY.    LBC 

0  -8.201034e-07 

1  -8.181804e-07 

2  -8.176153e-07 

3  -8.158862e-07 

4  -8.144222e-07 

5  -8.130164e-07 

6  -8.115498e-07 

7  -8.106662e-07 

8  -8.090450e-07 

9  -8.076453e-07 

10  -8.066382e-07 
DIFF. 


X 

0.965307 
0.965408 
0.965510 
0.965611 
0.965713 
0.965814 
0.965916 
0.966017 
0.966118 
0.966220 
0.966321 


Y 

0.659279 
0.659363 
0.659446 
0.659530 
0.659614 
0.659697 
0.659781 
0.659864 
0.659948 
0.660032 
0.660115 


TH1 

100.0000  • 

99.990615 

99.982685 

99.974756 

99.966828 

99.958901 

99.950974 

99.943049 

99.935124 

99.927201 

99.919278 


TH2 

110.0000 
•109.9958 
■109.9842 
-109.9726 
-109.9611 
-109.9535 
-109.9460 
-109.9364 
-109.9248 
-109.9133 
-109.9017 


TH3 

30.0000 
30.0026 
30.0034 
30.0043 
30.0051 
30.0059 
30.0067 
30.0076 
30.0084 
30.0092 
30.0101 


TH4 

30.0000 
30.0025 
29.9980 
29.9936 
29.9891 
29.9847 
29.9802 
29.9757 
29.9713 
29.9668 
29.9624 


0.001014  0.000836  0.000836    0.08073      0.0101      0.0376 


Weighted  Pseudo-Inverse  Equation 


CY 

0 
1 
2 
3 
4 
5 
6 
7 
8 
9 
10 


LBC 

3.614851e-01 
3.567477e-01 
3.517397e-01 
3.464733e-01 
3.409614e-01 
3.352178e-01 
3.292568e-01 
3.230936e-01 
3.167435e-01 
3.102225e-01 
2.945564e-01 


X 

0.965307 
0.965408 
0.965510 
0.965612 
0.965714 
0.965816 
0.965919 
0.966021 
0.966124 
0.966227 
0.966329 


Y 

0.659279 
0.659363 
0.659446 
0.659530 
0.659613 
0.659695 
0.659778 
0.659861 
0.659943 
0.660025 
0.660107 


TH1 

100.0000 
100.1065 
100.2098 
100.3082 
100.4016 
100.4899 
100.5733 
100.6515 
100.7246 
100.7926 
100.8555 


TH2 

-110.0000 
-109.9503 
-109.8983 
-109.8115 
■109.7200 
-109.6239 
-109.5233 
-109.4384 
-109.3493 
-109.2962 
-109.2594 


TH3 

30.0000 
29.3482 
28.7963 
28.0469 
27.2007 
26.2586 
25.3914 
24.6900 
23.0649 
22.2070 
21.3369 


TH4 

30.0000 
30.7754 
31.1421 
31.9064 
32.8677 
33.6252 
34.4785 
35.2269 
36.0697 
36.7066 
37.5437 


DIFF. 


0.001022  0.000828     0.8555        0.7406      -8.6631     7.5437 
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Fig.  3-14:  Initial  Configuration  of  a  Planar  GE  4R  Manipulator 
(e,  =  100°,  e2  =  -110°,  e,  =  30°,  e4  =  30°) 


56 


Fig.  3-15:  Implementation  Result  after  10  Cycles  with  the  General  Pseudo- 
inverse  Equation  (Final  Configuration:  9,  =  100.03°,  e2  =  - 
110.09°,  e,  =  30.0°,  e4  =  30.01°) 


Fig.  3-16:  Implementation  Result  after  10  Cycles  with  the  Weighted  Pseudo- 
inverse  Equation  (Final  Configuration:  e,  =  100.75°,  e2  =  - 
109.49°,  e,  =  21.12°,  e4  =  37.6°) 


CHAPTER  4 
REPEATABILITY  OF  A  SPATIAL  7R  MANIPULATOR 


The  proposed  general  right  pseudo  inverse  method  in  the  chapter  2  is  also 
applied  to  a  spatial  7R  manipulator.  It  is  therefore  necessary  to  develop  its  forward 
analysis. 

4.1  Notation 

The  notation  used  throughout  this  analysis  is  that  developed  by  Duffy  [12].  Briefly 
stated,  a  manipulator  is  composed  of  a  series  of  rigid  links.  One  such  link  is  shown 
in  Fig.  4-1.  In  this  figure  it  is  shown  that  the  link  connects  the  two  kinematic  pair 
(joint)  axes  Sj  and  Sj.  The  perpendicular  distance  between  the  pair  axes  is  a4j  and  the 
vector  along  this  mutual  perpendicu!  s  ay.  The  twist  angle  between  the  pair  axes 
is  labelled  a4  and  is  measured  in  a  right  handed  sense  about  the  vector  a^. 

The  particular  kinematic  pair  under  consideration  is  the  revolute  joint  which  is 
shown  in  Fig.  4-2.  The  perpendicular  distance  between  links,  or  more  specifically  the 
perpendicular  distance  between  the  vectors  ay  and  ajk,  is  labelled  as  the  offset 
distance  Sj.  The  relative  angle  between  two  links  is  shown  as  8j  and  is  measured  in 
a  right  handed  sense  about  the  vector  Sj. 

4.2  Mechanism  Dimensions  of  7R  Manipulator 

Shown  in  Fig.  4-3  is  a  kinematic  model  of  the  7R  Manipulator  (SSRMS).  In  the 
kinematic  model  the  joint  axes  are  labeled  sequentially  with  the  unit  vectors 
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Fig.  4-1:  Spatial  Link 
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Sjk 


9u 


Fig.  4-2:  Revolute  Pair 
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Fig.  4-3:   Kinematic  Model  of  the  7R  Manipulator  (SSRMS) 
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Sj  (i  =  1,  2,  ...  ,  7).  The  directions  of  the  common  normal  between  two  successive 

joint  axes  S;  and  Sj  are  labeled  with  the  unit  vectors  a^  (ij  =  12,  23,  ... ,  78). 

As  previously  stated,  the  link  lengths  a^,  the  offsets  Sj,  and  the  twist  angles  o^  are 
constants  which  are  specific  to  the  geometry  of  a  particular  manipulator.  The  values 
of  these  constants  are  tabulated  in  Table  4-1  for  the  SSRMS.  In  addition  to  the 
constant  dimensions,  S7  and  a7g  are  selected  such  that  the  point  at  the  end  of  vector 
a78  is  the  point  of  interest  of  the  tool  connected  to  the  manipulator.  For  example  this 
point  may  be  the  tip  of  a  welding  rod  that  the  manipulator  is  moving  along  a  path. 
Once  a  particular  tool  is  selected,  constant  values  for  S7  and  a7g  are  known. 

Furthermore  it  is  noted  that  the  link  lengths  a^,  a67  equal  zero.  However  it  is  still 
necessary  to  specify  the  direction  of  the  unit  vectors  au,  a^  in  order  to  have  an  axis 
about  which  to  measure  the  corresponding  twist  angles.  The  vector  ay  must  be 
perpendicular  to  the  plane  defined  by  the  vectors  Sj  and  Sj  and  as  such  can  have  two 
possible  directions.  For  the  vectors  an,  a^  this  direction  is  arbitrarily  selected  as  the 
direction  parallel  to  the  vector  SpcSj.  The  values  for  the  corresponding  twist  angles 
a^,  a67  listed  in  Table  4-1  are  determined  based  upon  this  convention. 
4.3  Specification  of  Position  and  Orientation 

The  first  step  of  the  analysis  is  to  establish  a  fixed  coordinate  system.  For  this 
analysis  a  fixed  coordinate  system  is  established  as  shown  in  Fig.  4-4  such  that  the 
origin  is  at  the  intersection  of  the  vectors  St  and  S^.  The  Z  axis  is  chosen  to  be 
parallel  to  the  Sx  vector  and  the  X  axis  bisects  the  allowable  range  of  rotation  of  the 
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Table  4-1 
Mechanism  Parameters  for  SSRMS 


S,  =  0  m  au  =  0  m  au  =  90° 


S2  =  0.635  a*  =  0.38  a*  =  270 


S3  =  0.504  aM  =  6.85  aM  =  0 


S4  =  0.504  a45  =  6.85  a*  =  0° 


S5  =  0.504  a56  =  0.38  a56  =  90° 


S6  =  0.635  a67  =  0  a67  =  90° 


S7  =  0.300*  a78  =  0.1*  a78  =  90° 


S7,  a78:  Tool  Dependent  Parameters 
a78:  an  Arbitrary  Twist  Angle 


Fig.  4-4:  Hypothetical  Closure  Link 
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angle  <pv  Throughout  the  rest  of  this  analysis,  this  coordinate  system  will  be  referred 

to  as  the  fixed  coordinate  system. 

Using  this  fixed  coordinate  system  it  is  possible  to  specify  the  position  and 

orientation  of  the  end  effector  by  specifying  the  vector  to  the  tool  tip,  Rp,  (see  Fig. 

4-4)  and  the  pair  of  orthogonal  unit  vectors  S^  and  a78.  Although  Rp,  a7g  and  S^  have 

a  total  of  nine  components,  the  latter  two  are  related  by  the  three  conditions, 

^78  "3:78    =    1> 

2mS*    =    0,  (4.1) 

^8*^8    =    1) 

so  that  the  three  vectors  (Rp,  a78,  SJ  represent  the  9-3  =  6  independent  parameters 
necessary  to  locate  a  rigid  body  in  space  (see  [13]). 

4.4  Derivation  of  Forward  Kinematic  Equation 
The  vector  loop  equation  of  the  equivalent  closed  loop  spatial  mechanism  in  the 
first  coordinate  system  can  be  written  by  (see  set  1  in  Table  4-2,  [12]) 

B/>  =    $&«  +  a23a23(1)  +  S&™  +  a^'1'  +  S&™  +  a45a45<1)  +  S&w 
+  W1*  +  S&m    +  S7S7(1)  +  a^w  (4.2) 

Q2  =      [0,  Su,  Cu]   , 

=   [0,-1,  Of,  (4.3) 

2*m        =    [c2,  U21\  U21]T, 

=    [c2,  0,  s2]T,  (4.4) 

S3  =    [X2  ,  Y2  ,  Z2  ]  , 

=    [-s2,  0,  c,]7-,  (4.5) 
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Table  4-2 
Direction  Cosines  -  Spatial  Octagon 


set  1: 

&  (  0,  0,  1  ) 

52  (  0j  "Sm  cn  ) 

53  (  X2  ,  Y2  ,  Z2    ) 

^4  \  A32»     *32>   ^32   ) 
Ss   (  -^432)   Y432,   Z432  ) 
«6  (.  "^5432>     *5432>   ^5432   / 
M7  (  -^65432)    *  65432*   ^45432  ) 
^8  (  -*765432»     *765432>   ^765432   ) 

set  8: 

s»  ( 0, 0, 1 ) 

Si  (  0,  -s81,  cgl ) 

52  (  X,  ,  Yj  ,  Zx    ) 

53  (  X21,  Y21,  Z21  ) 

54  (.  •^•321'    ^321*  ^321  ) 

55  (  ^M321>     *4321>   ^4321  ) 
S<>  (  -X-5432U    *  5432U  ^54321  ) 
S7  (-X*54321j     145432!,   Tsf^yiV 


&3 


«34 


§45 


^56 


^7 


&78 


^81 


S81 


112 


S23 


fi34 


S45 


S56 


S67 


1, 0, 0  ) 

^i  u21 ,  U21  ) 

"32>  "*^321  ,   U321   ) 
"4321   "U432i  ,    U4321  ) 
"5432*   "U 54321  >    '-'54321  ) 
"  65432>   "U  654321  >    U654321  ) 
W765432,   -U7654321  ,    U7654321   ) 

Ci,  -sl5  0  ) 
1,  0,  0  ) 

ClJ     U81    ,     UgJ    ) 

w21,  -U21\  U21  ) 

"321>  "U321  ,    U321  ) 
"4321>   "U432i  ,    U432i   ) 
"54321>   "*J5432i  ,    U54321   ) 
"654321*   "^654321  >    U654321  ) 


S78       (        "765432H       ■U7654321  ,       U7654321) 


(!)  =     fY       V       7    1T 
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£34  =      L  "32»  "U321  »  U321J   , 

=    [c2c3,  s3,  s2c3]T,  (4.6) 

M4  =      L-^32>    ^32>  ^32J    » 

-    K,  0,  erf,  (4.7) 

^45  =      I  "432>   "U4321  >    U4321J    , 

=      LC2C3  +  4>  S3+4>  S2C3  +  4j    »  V+-o) 

H5  =      l-^-432>     *432>  ^432j    > 

=    [-s2,0,c2]T,  (4.9) 

S56  =  l  "5432>  "^54321  >    LI 54321 J    , 

=  lC2C3+4+5>   S3  +  4+5>  S2C3  +  4  +  5j    >  (4.10) 

^6  =  L-^-5432>     *5432>   ^5432 J    > 

=  lC2S3  +  4  +  5>   "C3  +  4  +  5»   S2S3  +  4  +  sJ  V*-H) 

^7  =  1^65432'     *65432>   ^65432  J    » 

[s6C2C3+4+5  +  s2c6,  s6s3+4+5,  s6s2c3+4+5  -  C2CJ  ,  (4.12) 

^78  =      I" 765432'   "'-'7654321  >    U 7654321 J    » 

[-S2S6C7+C2(S7S3+4+5  +  C6C7C3+4+5),       "S7C3+4+5  +  C6C7S3+4+5,       C2S6C7 

+  s2(s7S3+4+5  +  c6c7C3+4+5)]T,  (4.13) 

Substituting  eq.  (4.7)  through  (4.17)  into  eq.  (4.6)  yields 

Rw  =  pya  RPy(1)'  VT,  (4.14) 

where 
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R  =     a23C2    -    S3S2     +     d-M^i    '    S4S2     +     245C2C3  +  4    "    °5S2     +     a56C2C3+4+5     "*" 


°6c2s3+4+s  +  S7(s6C3+4+5C2  +  c6s2)  +  a78(-s2s6c7  +  ^(5783+4+5  ■*" 

^^3+4+5)  )> 
Rpy     =  -S2  +  a34S3  +  a45s3+4  +  a56s3+4+5  -  0403+4+5  +  o7s6s3+4+5  +  a78(- 

S7C3+4+5    "*"    C6C7S3+4+5,)> 

RpZ(1)  =  a^  +  S3C2  +  aj^  +  S4C2  +  a^Sj^+^j  +  S5C2  +  aj^c^+j  + 

°6S2S3+4+5     +     °7(S6C3+4  +  5S2    "     C6C2)     +      *TV&frPl     +     S2(S7S3+4+5     + 
0607^3+4  +  5/   )> 

the  orientation  vector  of  end  effector, 

§8  =      IA765432>    ^765432>  ^7654321 

=      [02(068705  +  4+5      "      0783  +  4  +  5)      -      S2S6S7,      C6S7S3  +  4  +  5       +       C7C3+4+5, 

S2(C6S7C3  +  4  +  5  "  C7S3  +  4+5)  +   C2S6s7]T.  (4.15) 

Position  and  orientation  vectors  known  in  terms  of  the  first  coordinate  system  can 

be  found  in  terms  of  the  fixed  coordinate  system  as  follows: 

R„  =  [MHRW],  (4.16) 

a78  =  Mk.«l  (4.17) 

S,  =  [M][S,(1)],  (4-18) 

where 


M  = 


COS0! 

-sin0! 

0 

sin0x 

COS0! 

0 

0 

0 

1 

Detailed  derivation  of  these  kinematic  equations  for  the  position  and  orientation  of 
end  effector  is  presented  in  Appendix  C.  As  previously  stated,  although  eq.  (4.16), 
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(4.21),  and  (4.22)  have  a  total  of  nine  components,  eq.  (4.17)  and  (4.18)  are  related 
by  the  three  conditions  of  equation  set  (4.1)  so  that  eqs.  (4.16),  (4.17),  (4.18) 
represent  the  9-3  =  6  independent  parameters  necessary  to  position  and  orient  the 
end  effector  in  three  dimensional  space.  Throughout  the  remainder  of  this  paper,  Sj 
and  c,  will  represent  the  sine  and  cosine  of  6j  while  s^  and  c^  will  represent  the  sine 
and  cosine  of  ay.  The  terms  si+j  +n  and  ci+j  +n  represent  the  sine  and  cosine  of  the 
angle  fa  +  6j  +  . . .  +  Gn). 

4.5  The  Derivation  of  Jacobian  Matrix 

Fig.  4-4  shows  a  coordinate  system  attached  to  ground.  All  line  coordinates  will 
be  found  in  terms  of  a  coordinate  system  which  is  attached  to  the  SSRMS  and  has 
its  origin  located  at  the  intersection  of  the  vectors  S«  and  a78.  The  Z  axis  of  this 
coordinate  system  will  be  along  Sg  and  the  X  axis  will  be  along  a78.  The  Plucker 
coordinates  for  each  of  the  seven  axes  of  the  SSRMS  will  now  be  found  in  terms  of 
this  new  coordinate  system-eighth  coordinate  system.  It  is  important  to  recognize 
that  the  conditions  for  the  linear  dependency  of  the  line  coordinates  are  independent 
of  the  choice  of  coordinate  system. 

The  Plucker  coordinates  of  each  of  the  seven  joint  axes  are  comprised  of  the 
direction  of  the  joint  axes  and  the  moment  of  that  line  about  the  origin  of  the 
coordinate  system.  The  directions  of  each  joint  axis  measured  with  respect  to  the 
current  coordinate  system  are  listed  directly  as  set  8  in  Table  4-2.  It  has  also  been 
shown  in  Duffy  [12]  that  the  moment  of  each  line  about  the  origin  is  equal  to  the 
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dual  of  the  direction  vector.   The  Pliicker  axis  coordinates  of  the  seven  axis  lines, 

ii=[Soi;Si]T,  are  listed  as  follows: 

1(8)  =  [(-a78a78(8)  -  S7S7<8>  -  S«S,®  -  «5A.W  ■  *&m  -  *«bsm  "  S^  -  W> 

-  S&™  -  W0  -  s£2{S))x&r  I  &w  ]T,  (4-19) 

I2(8)  -  [(-a7Sa78(8)  -  S7S7<8>  -  SSF  -  W>  -  SA®  -  a«a««  -  S^  -  a^ 

'  5>3^3       "  ^232^23     )XO|2        J  ^        J    , 

i3(8)  =  [(-a78a78(8)  -  S7S7<8>  -  S&®  -  W>  -  S^  -  W>  -  S£f>  -  a^ 

)xS3(8)  ;  &«  ]T, 
1(8)  =  [(-aA(!)  -  S7S7<8>  -  S&v  -  a56a,6(8>  -  S&<»  -  a^®)^  I  S*®  P". 
*sW  -  [(-a78a78(8)  -  S7S7<8>  -  S&«  -  WW>  ;  &w  ]T, 
k(8)  =  [(-a78a78<8>  -  S^^W*  ;  S/>  ]T, 

t  («)  -  r.a    a    (8)  x  S  (8)  •  S  (8)  1T 

17        —    I  d78278       A  ^7        J  ^7       J   > 

The  Jacobian  matrix  can  be  expressed  as 

[J]  =  &(8)  &(8)  ia(8)  i/8)  £(8)  l(8)  hml  (4-2°) 

The  Pliicker  coordinates  for  each  of  the  seven  lines  in  eq.  (4.19)  can  be  simplified 
by  substituting  the  actual  SSRMS  mechanism  parameters  from  Table  4-1  into  the 
equations.  The  simplified  Pliicker  axis  line  coordinates  are  as  follows: 

$1  =  [  Soix  ^Oly  \lz  I  ^lx  Sjy  O^J  ,  V+-ZJJ 

£.2  =  L  ^02x  ^02y  ^02z   >   ^2x  ^2y  ^2zJ  > 

$3  =  I  ^03x  ^03y  ^03z  '■>  ^3x  ^3y  ^3zJ  > 

$4  =  I  ^04x  ^04y  ^04z  *>  \x  \y  ^4zJ  > 

is  =  I  \sx  ^05y  ^05z  5  ^5x  ^5y  ^5zJ  » 
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*6         =       L    ^06x  ^06y  ^06z   >   ^6x  S6y  06z]    , 
*7        =       L   \7x  ^07y  ^07z  >  ^7x  Sty  S7z]   , 

Soix  =  {-a^iCz  +S2Cj  +  SaSjSj  -  a34(s1c2c3  +  CjS3)  +  848^2  -  a45(s1c2c34  +  qs^)  +  S^Sj  - 

a56VSlC2C345    +     ClS34s)     +     S6(C1C345    -    SjCjS^)    -    S7(S1S6C345C2    +     SXC6S2    +     C^S^j)     + 
a78(SlS2S6C7  "  S1C2(S7S345    +    C6C7C345)    +    C1S7C345  "  ClC6C7S34s))}» 

soiy  =  (a23ciC2  +S2Sj  -  S3c1s2  +  a34(c1c2c3  -  s^)  -  S,,^  +  a45(c1c2c34  -  8,8*)  -  S^Sj  + 

ajeCCi^C^  -  SjSus)  +  Ss(S1Cj4S  +  CjCjS-^)  +  S^CiSjC^oC,  +  CjC6S2  -  s^s^j)  +  a78(- 
CjSjSjC,  +   C1C2(S7S345  +   C6C7C345)   +  S1S7C345  -  S^CtS^))}, 

Sou  =  0,       Su  =  0,       Sly  =  0,       S^  =  1, 

^02x  =  \"a23cis2  "  S3C!C2  -  a34c1s2c3  -  S4C1C2  -  a45c1s2C34  -  SjC^  -  a56c1s2c345  -  S6c1s2s345  - 
S7(c1s6c34Js2  -  CjC6c2)  -  a78(c1c2s6c7  +  c1s2(s7s345  +  ^070345))}, 

^02y    =:     l"a23SlS2    "    ^>3S1C2    "    a34SlS2C3    "    ^SjCj    -    a45S1S2C34    -    S^fo    -    a56S1S2C345    -    S6S1S2S345    - 

o7(s1s6c345s2  -  SjC^cJ  -  a7g(s1c2s6c7  +  s1s2(s7s345  +  c6c7C345))}, 
So2z  =  \a23c2 "  S3s2  +  Syfifo  -  S4s2  +  345(^034  -  S5s2  +  a^c^  +  S6C2S345  +  S7(S6C345C2  + 
C6S2)  +  a^C-SjS^c,  +  02(878345  +  c6c7C345))}, 

^2x    =    Sl>  S2y    =    -C1(  S2z    =    0, 

S03x  =  {-a34(CiC2s3  +  Sj(^)  -  a45(c1c2s34  +  s^)  -  a56(c1s2S345  +  SjCj*)  +  S6(c1c2c345  -  sa*)  - 
o7(c1s6S345c2+SiS6C345)  +  a7g(c1c2(s7c345  -  c6c7S345)  -  s1(s7s345  +  c6c7C345))}, 

S03y    =     {-a34(SlC2S3  -  CjCj)  -  a45(s1C2S34  -  CjC^)  -  a^S^  -  CjO    +    S6(S1C2C345    +    C^)   - 

S7(SiS6S345C2  -  c1s6c345)  +  a78(s1c2(s7C345  -  c6c7S345)  +  q(sA«  +  W^))}, 

So3z    =     \"a34S2S3  "  a45S2S34  "  a56S2S345    +    S6S2C345  -  S7S6S345S2)    +    a^Sj^C^  -   C6C7S345))}, 

5>3x  =  -C^,         o3y  =  Sfo,         o3z  =  C2, 
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So4x  =  {-a45(CiC2s34  +  s^)  -  ^(qCjS^  +  SiC^)  +  S^qc^  -  SA*)  -  S^c^s^  + 

S1S6C345)    +    a78VClC2(S7C345  "  C6C7S345)   "  Sl(S7S345    +    C6C7C34s))}> 


345 


345 


So4y  ■  {-a45(s1c2s34  +  qc^)  -  ^foc^  +  qc-^)  +  S^s^c^  +  c&J  -  S^s^s^c,- 
CiS6C345)  +  a7g(s1c2(s7c345  -  c^s^)  +  c^s^  +  c^c^))}, 

^04z    =       l"a45S2S34  "  a56S2S345    +    S^Cj^  -  S^S^jSj    +    a78S2VS7C345  "   C6C7S345)}> 
\x    =    "C1S2>  \y    =    "S1S2>  ^4z    =    ^2» 

S05x  =  {-asiCCi^Sj^  +  s^^)  +  S^CjCjC^  -  s^)  -  S^c^s^  +  s^O  +  ^c^StC 

"  C6C7S345)   "  Sl(S7S345    +    Ce0?^))}* 

S05y  ■  {-a56(SiC2S345  -  qc^)  +  S^SjC^  +  cfa)  ■  S7(s1s6Sj45c2-c1s6c345)  +  a78(s1c2(s7c 

C6C7S34s)    +    Cl(S7S345    +    ^C?^))}' 
^05z    =     \"a56S2S345    +    ^>6C2C34S  '  S7S6S345S2    +    ^S^S^^j  -   CgCjS^)}, 

o5x  =  "CjS^        o5y  =  -s1s2,        S5z  =  q, 

So6x  =  {S7(c1c6C345c2  -  qs^  -  s^s^)  -  a78(c1s2c6c7  +  c1c2s6c7c345  -  s1s6c7s345)}, 

^06y    =     \^7(S1C6C345C2  "  S1S6S2    +    ClC6S34s)   "  a78VSlS2C6C7    +    Sl^^^C^    "*"    C1S6C7S345)}> 

^06z    =     \S7(C6C345S2    +    ^^    +    ^■1»\C2C6C1  "  ^ff^3Wf> 

\x    =    C1C2S345    "*■    S1C345>  ^6y    =    S1C2S345  "  C1C345>  Sfe    =    S2Sj45, 

^07x    =     a78(ClS2S6S7    +    C1C2(C7S345  "  C6S7C345)    +    Sl(C7C345    +    C6S7S345))> 

So7y    =     a78(SlS2S6S7    +    S1C2VC7S345  "  C6S7C345)   "  Cl(C7C345    +    C6S7S345))> 

^07z    =     a78("C2S6S7    +    S2(C7S345  "  C6S7C34s))> 

^7x    =    C1C2S6C345    +    C1S2C6  "  S1S6S345>  Syy    =    SjC^C^    +    SjS^    +    C^S^, 

$7z    =    S2S6C345  "  C2C6- 
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A  detailed  derivation  of  Pliicker  axis  coordinates  of  the  seven  joint  axes  of  the  7R 

manipulator  (SSRMS)  is  presented  in  Appendix  D.  Now  the  Pliicker  axis  coordinates 

of  the  seven  joint  axes  of  the  7R  manipulator  (SSRMS)  are  known. 

4.6  The  Derivation  of  Inverse  Kinematic  Equation 

Joint  torques  are  related  to  joint  rotations  by 

[t,  t2  t3  t4  t5  t6  t7]t  =  [klfo-tfoj  e2-e02  e3-e03  e4-eM  e.-e^  e.-e^  e7-e07]T.     (4.22) 

where  ^-c^i  and  ei  -  e0i  (i  =  2,...,7)  are  differences  between  the  current  and  the  free 

position  joint  angle  and  [k]  is  a  7  x  7  diagonal  matrix  of  joint  stiffnesses  as  follows: 


[k]  = 


X 

0 

0 

0 

0 

0 

0 

0 

k2 

0 

0 

0 

0 

0 

0 

0 

k3 

0 

0 

0 

0 

0 

0 

0 

k4 

0 

0 

0 

0 

0 

0 

0 

k5 

0 

0 

0 

0 

0 

0 

0 

k6 

0 

0 

0 

0 

0 

0 

0 

k7 

(4.23) 


The  eq.  (4.22)  may  be  more  conveniently  written  as 

[01-001  e2-e02  ©3-©03  ©4-604  ^'©OS  ^©(K  e7-©07]T    =    WV 1    '2    U    U    * 5    U    ' 'if "•  (4.24) 

The  joint  torques,  acting  in  each  joint,  combine  to  give  forces  at  the  end  effector, 
and  this  can  be  expressed  as 

[t-i  r2  t3  t4  r5  t6  t7]t  =  [J]T[fx  fy  fz  mx  my  mf.  (4.25) 

Combining  the  stiffness  relationships  in  eq.  (4.24)  with  eq.  (4.25)  gives  the  joint 
motions 


[0r0oi  ©2-602  ©3-e03  e4-©04  e5-e05  e^  e7-e07]T  =  [k]1[J]T[fx  f,  fz  m,  m,  mJT.     (4.26) 
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Premultiplying  the  left  and  right  sides  of  eq.  (4.26)  by  [J]  and  rearranging  eq.  (4.26) 

yields 

[fx  fy  f,  mx  my  mJT  =  ([J][k]-1[J]T)-1[J][01-0ol  e2-e02 , , ,  e^e*  e7-e07]T.  (4.27) 

This  can  be  solved  provided  that  [J]  is  of  full  rank.  Taking  the  derivative  of  eq.  (4.26) 

yields 

[60!  se2  <se3  <se4  se5  6e6  <se7]T  =  \klVflSf,  <sfy  sfz  6mx  <Smy  *mJT  + 

[k]1[5J]T[fx  fy  fz  mx  my  <,  (4.28) 

where 


[<5J]T  = 


<5J 
<SJ 

61 
6J 
SJ 
61 
L«J17 


n 


12 


13 


14 


15 


16 


6J21 

<SJ31 

<5J41 

6J51 

5J22 

<SJ32 

SJ42 

<SJ52 

<5J23 

^33 

<5J43 

«SJ53 

<5J24 

6]^ 

SJ44 

SJ54 

Sj^ 

shs 

6J45 

«5J55 

«J26 

*J* 

5J46 

SJ56 

6J27 

6J37 

6J47 

*J57 

Si 
61 
61 
61 

si 
si 


61 


62 


63 


64 


65 


66 


61 
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6jpq    =    UP<M    +    Vpq5e2    +    Wpq5e3    +    UVp/G,    +    W^G,    +    UW^SG,;    +    UVW^SG,, 

p  =  1,  2,  ...,  6,  q  =  1,  2,  ...,  7. 

Detailed  derivation  of  u^,  vM,  ww,  uvw,  vw^,  uwN  and  uvw^  is  in  appendix  E. 
The  second  term  of  right  hand  side  of  eq.  (4.28)  goes  to  the  left  hand  side, 
[60x  6G2  5G3  6G4  6G5  6G6  667]T  -  [kJ-^Jftt,  fy  fz  mx  m,  mJT 


-  MWl<t  *,  Siz  Smx  6my  im,]1. 


(4.29) 


Substituting  eq.  (4.27)  into  eq.  (4.29)  and  rearranging  the  left  hand  side  of  eq.  (4.29), 
this  can  be  expressed  in  the  form 
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\T\[6tt  se2  se3  se4  se5  se6  se7]T  =  [kj-'pn^  Sf,  «4  <5n\  finiy  SmJ7,  (4.30) 
where  [T]  is  a  7  x  7  matrix  as  follows 


[T]  = 


ii 


l21 


l31 


l41 


l51 


l61 


'71 


x12 

T32 

T42 
T52 

T62 


13 


'23 


'33 


l43 


l53 


'63 


'73 


14 


L24 


l34 


l44 


l54 


LM 


'74 


15 


l25 


l35 


M5 


l55 


'65 


-75 


16 


'26 


36 


L46 


'56 


L66 


'76 


17 


'27 


'37 


l47 


'57 


•67 


■77 


Tpl  =  I  -  (SiUqpfq  +    Xu^mJ  /k,  (if  p  =  1,  I  =  1,  else  I  =  0,  p  =  1,2,...,7) 
Tp2  =  I  -  (s'v,  +    s'vqpm,)  /k2  (if  p  =  2,  I  =  1,  else  I  =  0,  p  =  1,2,...,7) 
Tp3  =  I  -  (z'w^  +   s'w^m,)  /k3  (if  p  =  3, 1  =  1,  else  I  =  0,  p  =  1,2,...,7) 
Tp*  =  I  -  (s'uvqpf,  +  S64uvqpmq)  /k4  (if  p  =  4,  I  =  1,  else  I  =  0,  p  =  1,2,...,7) 
Tp5  =  I  -  (r*vwJ5,  +  z'vwqpng  /k5  (if  p  =  5,  I  =  1,  else  I  =  0,  p  =  1,2,...,7) 
Tp,  =  I  -  (j'uwqpf,  +  s'uw^m,)  /kj  (if  p  =  6,  I  =  1,  else  I  =  0,  p  =  1,2,...,7) 

Tp7  =  !  "  (fl^  +  J4uvwqpmq)  /k7  (if  P  =  7, 1  =  1,  else  I  =  0,  p  =  1,2,...,7) 
Eq.  (4.30)  can  be  also  solved  provided  that  [T]  is  of  full  rank.  Thus, 

[60!  «e2  se3  6e4  se5  se6  <se7]T  =  [AjW^fx  sf,  <sfz  <smx  sn^  6mz]T.  (4.31) 
Letting 

[A]  =  [k][T], 
and  premultiplying  the  left  and  right  sides  of  eq.  (4.31)  by  [J]  yields 
[J][601  se2  se3  <se4  6e5  se6  se7]T  =  [JHAJWt-sfx  .sf,  sf,  5mx  Sm,  SmJ.      (4.32) 
The  forward  instantaneous  motion  equation  for  the  manipulator  is  given  by 


75 
[J][50!  se2  se3  <se4  se5  se6  5G7]T  =  [Sx  Sy  Sz  Sa  5/3  5y]T.  (4.33) 

Substituting  eq.  (4.33)  into  eq.  (4.32)  yields 

[Sx  Sy  Sz  Sa  5/3  5y]T  =  [J][A]1[J]T[5fx  Si,  «£,  im,  5m,  5mJT,  (4.34) 

which  can  be  expressed  in  the  form 

[Sx  Sy  Sz  Sa  5/3  5y]T  =  [C][5fx  6fy  S^  5m,  im,  5mz]T,  (4.35) 

where  [C]  =  [J][A]"1[J]T  is  an  6  x  6  symmetric  compliance  matrix  that  relates  the  end 
effector  external  wrench  to  its  corresponding  twist.  Provided  that  [T]  and  [J]  are  both 
full  rank,  then 

[5fx  Sfy  Siz  Smx  Smy  5m2]T  =  [C]_1[6x  5y  Sz  Sa  5/3  5y]T,  (4.36) 

relates  the  necessary  deflection  twist  of  the  end  effector  relative  to  ground  to  the 
incremental  change  in  resultant  wrench. 

In  the  problem  of  the  inverse  kinematics  of  the  redundant  serial  manipulator,  it 
is  required  to  determine  the  joint  increments  explicitly.  The  elastic  compliance 
relation  of  eq.  (4.35)  can  be  used  to  accomplish  this  objective.  Substituting  eq.  (4.36) 
into  eq.  (4.31)  yields  the  general  pseudo-inverse  equation: 

[50j  5G2  593  504  505  596  5G7]T  =  [A]1[J]T([J][A]1[J]T)"1[<5x  5y  Sz  Sa  5/3  5y]T 

=  [ArWtQHSx  5y  Sz  Sa  5/3  Sy)T.  (4.37) 
4.7  Numerical  Verification  and  Simulation  of  Repeatability 
Mechanism  dimensions  are  given  as  table  4-1.  The  joint  limits  are  selected  as 
follows: 

-180°  <  0!  <  180°,        -180°  <  e2  <  60°,         -180°  <  e3  <  180°, 
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-180°  <  e4  <  180°,        -180°  <  es  <  100°,  -180°  <  e6  <  180°, 

-180°  <  e7  <  120°. 
The  free  position  angles  and  the  stiffnesses  of  each  joint  can  be  chosen  as 

0O1  =  (-180  +  180)/2  =  0° ,        e^  =  (-180  +  60)/2  -  -60°, 

e03  =  (-180  +  180)/2  =  0° ,        eM  =  (-180  +  180)/2  =  0°, 

G05  =  (-180  +  100)/2  =  -40° ,     Go,  =  (-180  +  180)/2  =  0° , 

e07  =  (-180  +  120)/2  =  -30°, 

k,  =  360/360  =  1.0,     k2  =  360/240  =  1.5,       k3  =360/360  =  1.0, 

k4  =  (360/360  =  1.0,   k5  =  360/280  =  1.28,     k6  =360/360  =  1.0, 

k7  =  360/300  =  1.2  Nm/radian. 
The  initial  joint-angle  values  are  e  =  [190°,  190°,  190°,  190°,  190°,  190°,  190°]T. 
The  x-y  coordinates  of  the  four  vertices  of  the  command  path  (3000  consecutive 
equidistant  segments  horizontal  side  and  1000  segments  vertical  side)  in  the 
workspace  are  successively  given  counterclockwise  from  the  lower  left  vertex  as 
follows: 

(-0.7535,  -0.8966)      (-0.4535,  -0.8966) 

(-0.7535,  -0.9866)      (-0.4535,  -0.9866) 
where  the  units  are  meters.  The  end  effector  is  to  track  the  rectangular  path  with  a 
constant  speed  of  10  mm/sec,  a  constant  orientation  (a78x  =  -0.86994,  a78y  =  0.49171, 
a78z  =  -0.03704,  S8x  =  -0.49310,  S^  =  0.86615,  S82  =  0.86615),  and  a  time  step  size  At 
=  0.01  sec. 
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Lie  bracket  condition  values  through  5  cycles  of  the  motion  are  shown  in  Fig.  4-5. 
The  initial  and  final  joint  angles  through  each  cyclic  motion  of  the  end  effector  are 
generated  in  Table  4-3.  At  the  end  of  5  cyclic  motion  of  the  end  effector,  the  position 
difference  ((Rp)Dlial  -  (Sp)^)  and  the  joint  angle  difference  ((<Pi)raai  -  (<Pi)utu,  (*d** ' 
(ei)iniuai>  i  =  2,  3,  ,  ,  ,  7)  are  also  shown  in  Table  4-3.  By  the  orders  of  magnitude 
comparison,  it  is  clear  that  the  general  pseudo-inverse  control  (eq.  4.37)  is  repeatable 
in  practice  whereas  the  weighted  pseudo-inverse  control  (eq.  2.46)  is  not. 
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LBC    VS    CYCLIC    MOTION 


LBC:    LIE    BRACKET    CONDITION 


OOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOOO 


u 

CD 


-0.6 


0  12  3  4  5 

CYCLEC SYMBOL: GENERAL    PSEUDO- I NVERSE,    SOL  ID : WE IGHTED   PSEUDO- I NVERSE3 


Fig.  4-5:  Lie  Bracket  Condition  Values  Through  5  Cycles  of  the  Motion 
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CY.    LBC 

0  5.247532e-6 

1  5.251108e-6 

2  5.256534e-6 

3  5.261504e-6 

4  5.265164e-6 

5  5.269522e-6 
DIFF. 


Table  4-3 
Simulation  Results  Through  5  Cycles  of  the  Motion 

General  Pseudo-Inverse  Equation 
PHI1  TH2  TH3  TH4  TH5  TH6  TH7 
190.0000  190.0000  190.0000  190.0000  190.0000  190.0000  190.0000 
189.9600  190.0127  189.9615  189.9991  189.9996  190.0145  190.0005 
189.9205  190.0252  189.9234  189.9981  189.9992  190.0288  190.0011 
189.8817  190.0376  189.8858  189.9972  189.9989  190.0430  190.0018 
189.8435  190.0499  189.8487  189.9963  189.9986  190.0570  190.0025 
189.8047  190.0629  189.8124  189.9940  189.9972  190.0716  190.0045 
-0.1953    0.0629      -0.006       -0.006      -0.0028    0.0716      0.0045 


CY. 

0 
1 
2 
3 
4 
5 
DIFF. 


LBC 

5.247532e-6 
5.251108e-6 
5.256534e-6 
5.261504e-6 
5.265 164e-6 
5.269522e-6 


RPx 

-0.75357 
-0.75345 
-0.75333 
-0.75321 
-0.75309 
-0.75297 
0.00078 


Rpy 

-0.98664 
-0.98659 
-0.98654 
-0.98650 
-0.98645 
-0.98645 
0.00019 


Rpz 
-1.69162 
-1.69163 
-1.69163 
-1.69163 
-1.69163 
-1.69163 
0.00001 


CY.    LBC 

0  -7.080107e-01 

1  -7.064213e-01 

2  -7.011823e-01 

3  -6.923246e-01 

4  -6.871246e-01 

5  -6.826156e-01 
DIFF. 


Weighted  Pseudo-Inverse  Equation 

PHI1  TH2  TH3  TH4  TH5  TH6  TH7 
190.0000  190.0000  190.0000  190.0000  190.0000  190.0000  190.0000 
190.8676  189.7794  190.7038  189.9964  190.0549  189.7342  190.1011 
191.7274  189.5745  191.3862  189.9928  190.1164  189.4839  190.2099 
192.5792  189.3841  192.0502  189.9880  190.1820  189.2479  190.3269 
193.4232  189.2067  192.6974  189.9819  190.2508  189.0248  190.4515 
194.2600  189.0409  193.3296  189.9746  190.3220  188.8132  190.5831 

4.2600       -0.9591  3.3296      -0.0254    0.3220      -1.1868      0.5831 


CY.  LBC 

0  -7.080107e-01 

1  -7.064213e-01 

2  -7.011823e-01 

3  -6.923246e-01 

4  -6.871246e-01 

5  -6.826156e-01 
DIFF. 


Rpx 

-0.75357 
-0.75345 
-0.75334 
-0.75323 
-0.75311 
-0.75299 
0.00058 


Rpy 

-0.98664 
-0.98643 
-0.98637 
-0.98632 
-0.98626 
-0.98621 
0.00043 


-1.69162 
-1.69163 
-1.69165 
-1.69166 
-1.69167 
-1.69168 
0.00006 


CHAPTER  5 
CONCLUSION 


The  analysis  presented  here  has  provided  a  new  closed-form  solution  for  the 
inverse  instantaneous  kinematic  problem  of  any  serial  redundant  manipulator.  An 
essential  requirement  for  this  solution  is  to  provide  the  free  angles  (e0i)  and  spring 
stiffnesses  (kj.  Here  a  hypothetical  stiffness  matrix  was  constructed  using  joint  limit 
data  of  the  robot  manipulator.  Free  angles  of  torsional  springs  are  also  conveniently 
chosen  based  on  joint  limits.  This  procedure  yields  a  control  strategy  that  facilitates 
repeatability  in  the  joint  space  of  a  manipulator  whose  the  end  effector  undergoes 
a  cyclic  type  motion.  The  numerical  verification  for  repeatability  is  done  in  terms  of 
the  Lie  bracket  condition.  From  the  numerical  verification  and  simulation  results,  it 
can  be  said  that  the  general  pseudo-inverse  control  is  repeatable  in  practice  for 
different  initial  configurations  whreas  the  weighted  pseudo-inverse  and  the  Moore- 
Penrose  control  are  not.  The  implementation  for  repeatability  was  accomplished 
successfully  on  the  planar  GE  4R  manipulator. 

A  symbolic  proof  for  repeatability  will  be  a  future  research  work. 
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APPENDIX  A 

DERIVATION  OF  INVERSE  KINEMATIC  EQUATION  OF 

PRR  MANIPULATOR 


The  kinematic  model  of  a  planar  PRR  manipulator  is  shown  in  Fig.  3-10. 
The  coordinates  for  a  point  P  in  the  end  effector  are 

x  =  ssj  +  a^Cj  +  a^^  (A.1) 

y    =    a23S2    ■*"    a34S2+3 

The  Jacobian  matrix  is  obtained  as 


[J] 


1.0  "a23S2~a34S2  +  3  *a34S2  +  3 


.0.0  &23C2+ ^uCz  +  l  ^9fi2*3 


(A.2) 


The  joint  spring  force  acting  in  the  first  joint  and  joint  torques  acting  in  the  second, 
third  joints  combine  to  give  forces  at  the  end  effector,  and  these  can  be  expressed 


as 


=     [J]1 


L  % 


(A3) 


Joint  torques  are  related  to  joint  rotations  by 
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fl 

SSi  -  SSqi 

T2 

=  M 

G2  -  902 

T3. 

.  ©3  "   ®03 

(A.4) 


where  [k]  is  a  3x3  diagonal  matrix  of  joint  stiffnesses  as  follows 


[k]  = 


ki 

0 

0 

0 

k2 

0 

0 

0 

k 

ssj  -  ss01  is  the  difference  between  current  and  the  free  position  displacement  while 
Gj  -  e0i  are  the  difference  between  current  and  free  position  joint  angles,  i  =  2,  3. 
The  eq.  (A.4)  can  be  written  in  the  form 


SSj  -  ss01 

"fi 

62  -  602 

=  M1 

r2 

.       ©3  "  ®03 

.  T3 

(A.5) 


Substituting  eq.  (A.3)  into  eq.  (A.5)  yields 


ssx  -  s& 


01 


e, -e 


2        "02 


G3  -  G03 


lfTlT 


=    [k]1!!] 


fy 


(A.6) 


Multiplying  eq.  (A.6)  by  [J]  yields 
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[J] 


ssx  ■ 

ss01 

e2 

"   ©02 

e3 

-  ©03 

-irnT 


=    [WW) 


fy    J 


(A.7) 


Rewriting  eq.  (A.7)  yields 


fy 


=  ( [J][k]-w  r  m 


SSj  -  ss 


'01 


©2  "   ©02 


L  ©3  "  ©03 


(A.8) 


Taking  derivative  of  eq.  (A6)  yields 


SsSj 
<se2 
se3 


-irnT 


=    [k]1^] 


St 


**,    J 


If  rTlT 


+  [kn«j] 


L*U 


(A.9) 


where 


[sir  = 


Mi 


v31 


•^12 


Y21  ^22 


v32 


(A.10) 


Au  =  0.0,  Au  =  0.0 


A21  —  (-a23C2  -  83402+3)  602  -  334^+3  d©3 


A22    -    ("a23S2  "  a34S2  +  3)    ^©2  "  S34S2  +  3    «*©3 


A31 ^■■iA,C2^   *©2  "  a34C2  +  3    ^©3 


A32 a34S2  +  3    ^©2  "   a34S2  +  3    ^©3 


Rewriting  eq.  (A.9)  yields 


84 


fiSSi 


se 


3    J 


WW 


fy 


lrnT 


=  MV] 


61 


K 


(A.11) 


Rearranging  left  hand  side  of  eq.  (A.11)  yields 


where 


[T] 


fiSSi 

Lse3 


irnT 


=  MTJ] 


fif. 


L^  J 


(A.  12) 


m  = 


Tu       T, 


T21       T. 


13 


T 

22  x23 


l31 


1 32  *  33 


Tu  =  1.0,  Tn  =  0.0,       T13  =  0.0,  T21  =  0.0 

T22  =  1  -  {  (-a23C2  -  23492+3)  fx  +  (-aasSj  -  a34S2+3)  fy  }/k2 

T23  =  -  (  -Q-yfa-fi  *x     "  a34S2+3  *y  )/*2 

T31  =  0.0,  T32  =  -  ( -a34C2+3 1,  -  a34S2+3  f^  )/k3 


T33  -  1  -  ( -33402+3  fx  -  a34S2+3  fy  )/k3 


Rewriting  eq.  (A.  12)  yields 
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se2 
(Se3 


ln,i-irnT 


=  [TTWIJ] 


fit 


I  Si,  J 


(A.13) 


Multiplying  eq.  (A.13)  by  [J]  yields 


[J] 


6sst 
se2 

69, 


irui-irnT 


=  [JinrwiJ] 


st 


L    * 


(A.14) 


The  forward  instantaneous  motion  equation  for  the  manipulator  is  given  by 


[J] 


6ssx 

"6x 

692 

= 

.  «593  . 

.<5y  . 

(A.15) 


Substituting  eq.  (A.15)  into  eq.  (A.14)  yields 


"6x  " 

=  [Q 

sf, 

.fiy  . 

-  tt,  - 

(A.16) 


where 


lruvimT 


[q  =  [JimwiJ] 


Rewriting  eq.  (A.16)  yields 


st, 

-  [C]1 

"6x 

-  tt,  - 

.fiy  . 

(A.17) 
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Substituting  eq.  (A.  17)  into  eq.  (A.  13)  yields  the  general  pseudo-inverse  equation 


<5ss, 


69, 


66 


3    J 


=  mww(  [Jim-ww ) 


<5x 

L6y 


(A.18) 


in,virnTrr<i-i 


=  m-wm'tc] 


Sx 


«5yJ 


APPENDIX  B 

DERIVATION  OF  INVERSE  KINEMATIC  EQUATION  OF 

4R  MANIPULATOR 

The  kinematic  model  of  a  planar  4R  manipulator  is  shown  in  Fig.  3-12. 

The  coordinates  for  a  point  P  in  the  end  effector  are 

x  =  3-u^  +  a23C1+2  +  a34C1+2+3  +  a45c1+2+3+4,  vtf-v 

y    =    ai2Sl    "*"    a23Sl  +  2    "*"    a34Sl  +  2  +  3    "*"    a45Sl  +  2  +  3+4> 

a   =  Gx  +  G2  +   G3  +   64, 

where  au,  aM,  a^,  a45  represent  the  length  of  each  link.  Eq.  (B.l)  for  an  instantaneous 
motion  can  be  expressed  as 

[Sx  6y  6a]T  =  [JJ[66,  SQ2  693  564]T.  (B.2) 

The  Jacobian  matrix  is  obtained  as 


[J] 


Jn      J^       J13       J14 

J21  J22  J23  ■'24 

1.0      1.0      1.0       1.0 


(B.3) 


where 


•Ml    —    "ai2Sl  "  a23Sl  +  2  "  a34Sl  +  2  +  3  "  a45Sl  +  2  +  3+4» 


Jo  —  -a^-iSi .■)  -  a^Si .■).■>  -  a4cS 


'12 


233l  +  2  "  <l343l  +  2  +  3       a45al  +  2  +  3  +  4> 


J 13    ~    "a34Sl+2+3  "  a45Sl+2+3+4> 
J14    =    -a45S1+2+3+4, 


J„    = 


J21 


J„    = 


'22 


ai2Cl    ■*"    a23Cl+2    "*■    a34Cl+2  +  3    "*"    a45Cl  +  2  +  3+4> 
a23Cl+2    "*"    a34Cl+2  +  3    +    a45Cl  +  2  +  3+4» 
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^23    =    a34Cl+2+3    "*"    a45Cl+2+3+4> 
J24    =    <*45Cl+2+3+4> 

Joint  torques  acting  in  each  joint  combine  to  give  forces  at  the  end  effector,  and 
these  can  be  expressed  as 


t2 


=        [J]1 


(B.4) 


Joint  torques  are  related  to  joint  rotations  by 


*1 " 

©i  "  eoi 

T2 
T3 

=  [k] 

e2  -  ©02 

©3  "   e03 

.    u  . 

.©4  "   ©04     - 

(B.5) 


where  [k]  is  a  4x4  diagonal  matrix  of  joint  stiffnesses  as  follows 


[k]  = 


X 

0 

0 

0 

0 

k2 

0 

0 

0 

0 

k3 

0 

0 

0 

0 

k4 

©i  -  ©0i  are  differences  between  current  and  free  position  joint  angles,  i  =  1,  2,  3,  4. 


The  eq.  (B.5)  can  be  written  in  the  form 


Substituting  eq.  (B.4)  into  eq.  (B.6)  yields 


Premultiplying  the  left  and  right  sides  of  eq.  (B.7)  by  [J]  yields 


Rewriting  eq.  (B.8)  yields 


Taking  the  derivative  of  eq.  (B.7)  yields 
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©1 

■  ©01 

G2 

"  ©02 

e3 

"  ©03 

e4 

"  ©04 

=  [k]'1 


(B.6) 


©1  "  ©01 

"       fx 

©2  "  ©02 

-  WW 

*, 

©3  "  ©03 

.    rn0  . 

.       ©4  "  ©04 

(B.7) 


[J] 


©1- 

"  ©01 

©2 

"  ©02 

©3 

"   ©03 

©4"    © 


04 


-imT 


=    [JPHJ] 


4 

L  rn<, 


(B.8) 


mo  J 


=    ([JIM-1!!]1)-1  [J] 


ex  -  © 


01 


©2 

"   ©02 

©3 

"   ©03 

©4 

"   ©04 

(B.9) 
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56i 

5e3 
«e4  . 


-lnfr 


wiJ] 


+  MW 


(B.10) 


where 


[5J]T  = 


S3U 

6JU 

0.0 

SJ21 

6J22 

0.0 

shi 

<5J32 

0.0 

<5J41 

SJ42 

0.0 

S3U 

6J 


12 


5J„  = 


'21 
5J22  = 
5J31  = 
6J32  = 
6J41  = 
<5J42  = 


un5ei  +  Vii5^  +  wn*e3  +  uvu6e4, 
ui2S91  +  vn6Q2  +  v/n6e3  +  uwnSQ4, 
U215Gj  +  v21<562  +  w21<563  +  uv21<564, 
U^fiGi  +  \226&2  +  W^SGj  +  UV22<SQ4, 
U316G!  +  v315G2  +  w315G3  +  UV315G4, 
U32SQ1  +  V326G2  +  W32<563  +  UV325G4, 
U4lS01  +  v41<S62  +  w416G3  +  UV416G4, 
UA2SB1  +  v426G2  +  w425G3  +  uv42<564, 

Ull  =  "aUCl  "  a23Cl  +  2  "   a34Cl  +  2  +  3  "  a45Cl  +  2  +  3+4> 

Uj2  =  "  a23Cl+2  "  a34Cl+2  +  3  "  a45Cl  +  2  +  3  +  4> 

Uu  =  "  S34C1+2  +  3  -  345Cl  +  2  +  3  +  4>       U14    =    "  a45Sl  +  2  +  3  +  4> 

U21  =  -a^Sj  -  S23S1  +  2  -   &34S1  +  2  +  3  "   a45Sl  +  2  +  3+4» 


(B.ll) 


U22    =    *  a23Sl+2  "   a34Sl  +  2  +  3  "  a45Sl  +  2  +  3  +  4> 
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U23    =    "  a34Sl+2  +  3  "  a45Sl+2+3+4>    U24    =    _a45Sl+2+3+4> 

Vll    =    '  a23Cl+2  "  a34Cl+2  +  3  "  a45Cl+2  +  3+4> 

V|2    =    "  &23Cl+2  "  a34Cl  +  2  +  3  "  a45Cl+2  +  3  +  4> 

V13    =    "  a34Cl+2  +  3  '  a45Cl  +  2  +  3+4»  V14    =      "  a45Cl+2  +  3+4> 

V21    =    "  A23Sl+2  "  a34Sl+2+3  "  a45Sl+2+3+4» 

V22    =    "  a23Sl  +  2  "  a34Sl  +  2  +  3  "  a45Sl  +  2  +  3+4i 

V23    =    "  a34Sl  +  2  +  3  "  a45Sl  +  2  +  3  +  4>    V24    =    "  a45Sl  +  2  +  3+4> 

Wll    =    "  a34Cl+2  +  3  "  a45Cl+2  +  3+4»      W12    =    "  S34Cl  +  2  +  3  "   S45Cl+2+3+4» 

W13    =    "  a34Cl+2  +  3  "  a45Cl  +  2  +  3+4>       W14    =    "   a45Cl  +  2  +  3+4> 

W21    =    "  a34Sl+2  +  3  "  a45Sl+2  +  3+4>       W22    =    "  a34Sl+2+3  "  a45Sl+2+3+4> 

W23    =    "  a34Sl  +  2  +  3  "  a45Sl+2  +  3+4>       W24    =      "   a45Sl  +  2  +  3+4> 

UVn    =    -  345^+2  +  3+4,  UV12    =    "  a45Cl+2+3+4> 

UV13    =    "  a45Cl  +  2  +  3  +  4>  UV14    =    "  a45Cl  +  2  +  3  +  4> 

UV21    =    -  a45S1+2  +  3+4,  UV22    =    "  a45Sl  +  2  +  3  +  4> 

uv23  =  -  a45s1+2+3+4,  uv24  =  -  a45s1+2+3+4, 


Rewriting  eq.  (B.10)  yields 


59x 
se2 
6e3 
6QA 


-  MW 


% 

.m<, 


lrnT 


=  [k]x[J] 


«fx 

<Sm0 


(B.12) 


Rearranging  left  hand  side  of  eq.  (B.12)  yields 
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Where 


m 


<S93 

_se4 


-lrnT 


=  [kHJ] 


[T]  = 


a 


l21 


■31 


'41 


I22 

T 

A32 

T42 


Si, 

.^rrio  . 


13 


'23 


1  33 


L43 


14 


L24 


'34 


■44 


(B.13) 


T„  = 


■11 


13 


T21  = 

T31  = 

T33  = 

T41  = 

T43  = 


1  -  (  uufx  +  \xjy  )/k1,  Tu 

-(  wufx  +  w21f;  )/k1>  T14 

-(  u^f,  +  ujy  )/k2,  T^ 

-(  wufx  +  Wjj^  )/k2,  T24 

-(  u13fx  +  u23f;  )/k3,  T32 

1  -  (  w13fx  +  w23f;  )/kj,  TM 

-(  u14fx  +  u24fy  )/k4,  T42 

-(  w14fx  +  w24fy  )/k4,  T^ 


Rewriting  eq.  (B.13)  yields 


<592 

<S93 

_«S94 


lrnT 


=  IMV] 


-(  vafx  +v21fy  )/ku 
-(  uvu£,  +  uv2£  )/K, 
1  -  (  vufx  +  v^fy  )/k2, 
-(  uvufx  +  uv^f;  )/k2, 
-(  v13fx  +  v^  )/k3, 
-(  uvi3fx  +  uvjy  )/k3, 
-(  vJ4fx  +  v^  )/k4, 
1  -  (  uv14fx  +  uv24t;  )/k4, 


6mo 


(B.14) 


Premultiplying  the  left  and  right  sides  of  eq.  (B.14)  by  [J]  yields 
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[J] 


se2 
se3 
se 


4    J 


lrnT 


=    [J][A]J[J] 


Sty 
<5m„ 


(B.15) 


Substituting  eq.  (B.2)  into  eq.  (B.15)  yields 


~6x  ' 

si.  ' 

6y 

=  [C] 

* 

.6a  . 

.    5mo  - 

(B.16) 


where 


irnT 


[q  =  [jhahj] 


Rewriting  eq.  (B.16)  yields 


"    <5fx    ■ 

~6x 

* 

=    [C]1 

Sy 

.   5mo . 

.6a  . 

(B.17) 


Substituting  eq.  (B.17)  into  eq.  (B.14)  yields  the  general  pseudo-inverse  equation 


6e1 

602 

<se3 

.  6e4 


=  [A\vn  vmw ) 


6x 
Sy 

6a 


(B.18) 


APPENDIX  C 

DERIVATION  OF  FORWARD  KINEMATIC  EQUATION  OF 

7R  MANIPULATOR  (SSRMS) 


The  vector  loop  equation  of  the  equivalent  closed  loop  spatial  mechanism  in  the 
first  coordinate  system  can  be  written  by  (see  set  1  in  Table  4-2,  [12]) 

Rp(1)  =   S^  +  a^  +  S33S3(1)  +  a^  +  S^  +  a^  +  §»&«  + 

2^s6m  +  S«S,(I)   +  S,&«  +  a78a78(1)  (CI) 

S^-0,  S/)  =  -Sl2,  S^  =  Cl2  (C.2) 

a23x(1)  =  c*  a^J  =  U21\  a23«  =  U2l  (C.3) 

U21   =  s2cu,  U21  =  s^^ 

S3X(1)  -  X2  ,  S3y(1)  =  Y2  ,  S3z(1>  =  %  (C4) 

X  =  S^Sj,  Y2=  "  (  S'rihs  +  C12S23p2  ) 

Z2=    CiaC23  "  S12S23C2 

a*.*'  -  W32,  a*«  =  -IV,  ■««  =  U321  (C.5) 

U321  =  U^  -  V^Su,      U321*  =  U32sn  +  V32cn 
U32  =  s3s23,  V32  =  -  (  s2c3  +  CjSjCb  ) 

32    —    4C3  ■  S2S3C23 

S4X(1)  =  X32,  S4«  =  Y32,  S42«  =  Z32  (C.6) 

X32  =  X3  c,  -  Y3  s2,        Y32  =  cu(X3  s2  +  Y3  c2)  -  s^Z, 
Z32  =  su(X3  s2  +  Y3  cj  +  cuZ3 
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X3       =    S34S3,  Y3       =    -  (  S23CJ4    +    C^S^Cj  ) 

^3        =    ^23^34  "  ^23S34C3 

a45x(1)  =  W432,  ^  =  -U4321\  a45z(1)  =  U4321  (C.7) 

^4321    =    ^^C^  "  V 432Sn,      U4321      =    U^S^    +     V^jC^ 

U432  =  U431C23  -  V43s23 

V432    =    <*(  U^Sjj    +    V^Cy  )   -  S2W43 

W432  =  s2(  U^  +  V43C23 )  +  c2W43 

U43  =  s^,  V43  =  -  ( s3c4  +  C3S4C34  ) 

"43    =    C3C4  "  S3S4C34 

S5X(1)  =  X432,  S*«  =  Y432)  S^  =  Z432        (C.8) 

X432  =  X43c2  -  Y43s2,  Y432  =  (^(X^jSj  +  Y43C2)  -  s^Ao 

Zj32    =    S12(^43S2    +      *43C2)    +    C^Z,^ 

X43  =  X4  c3  -  Y4  s3  Y43  -  c23(X4  s3  +  Y4  c3)  -  s23Z4 

Z43    =    S23V^4    S3   +     M    C3)    +    C^LA 

X4        =    S45S4,  Y4        =    -   (  S^C^    +    CjjS^C,,  ) 

Z4        =    C34C45  -  S^S^Q, 

a5,x(1)  =  W^,  n**  =  -  UM321\  a56«  =  U^    (C.9) 

W5432    =    S2(  U^S,,    +    Vj^Cjj  )    +    (^W^ 

U^^i      =    ^5432*12    +    ^5432C12'  U^^j    =    U^^C^  -    V  5432s  U 

U5432    =    ^543C23  "    » 543S23 

^5432    =    ^V    U543S23    +    V543C23  )   -  SjW^j 

U543    =    U^Cj,  -  V^S*.  V543    =    C,(  U^S*    +    V^Cj,  )   -  S3WM 
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W^    =    S3(  U^S*    +    V^  )    +    C3WM 

U^  =  s5s45,  Vj4  =  -  (  s4c5  +  c4s5c45 ),      W^  =  c4c5  -  s4s5Cs4 

S6X(1)  =  XM32,  S6«  =  YM32  Sfc«  =  ZM32  (CIO) 

^5432    =    Xs43C2  -  ¥54383  *5432    =    C12(^543S2    +      *  Mlfhj   '  ^U^SAl 

^5432    =    S12(X543S2    +     *543C2)    +    C12^543 

^■543    =    Xs4C3  -     15483,  YM3    =    C23(X54S3    +      1 54C3)   "  823/^54 

Z543    =    S23(XmS3    +    ^54C3)    +    ^^54 

Xj,  =  X5  c4  -  Y5  s4,        YM  =  03,(5(5  s4  +  Y5  c4)  -  s^ 

ZM  =  s^X,  s4  +  Y5  c4)  +  C34Z5 

X5     =  s56S5,  Yj     =  -  (  s45c56  +  C45s56Cj  ) 

Z5     =  c45c56  -  S45S56C5 

S^    =    X.5432,  S^    =    Y.5432,  S,»-2^.       (Gil) 

X&5432    =    ^6SA3C2  -  Y6543S2)         Y65432    =    Cj^X^jSj    +      I  SMS^)   "   S12^J6SA3 

Z65432    =    S12(X6543S2    +     *fS>efcu    "*"    CU^65Ai 

X6543    =   X^Cj  -  Y^j,  Y6543    =    C23(X654S3    +    Y^)   -  SjjZ^ 

^6543    =    S23(-^654S3    +      *654C3)    "*"    C23^654 

X^  =  X^c,  -  Y^,         Y^  =  C34(X65s4  +  Y^)  -  s^Z, 

^654    =    S34(-^d5S4    "*"     ^65C4)    +    C34^65 

X^  =  X,  c5  -  Y6  s5)  Ya  =  c45(X6  s5  +  Y6  c5)  -  s45Z( 

Z«s  -  s45(X6  s5  +  Y6  c5)  +  c^Zj 
X6    =  s67s6,  Y6    =  -  (  s56c67  +  c56s67c6 ) 

^6    =  c56c67  -  s56s67c6 
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a78x     =  w765432,         a7gy     =  -  U7654321 ,      a78z     =  U7654321  (C.12) 

*-)7654321    =    U765432C12  -    V765432S12 

U  7654321      =    L^^^S^    +    V-j^j^C^ 

"765432    =    S2(   U76543S23    +     V76S43<h3  )    +    C^W^^-j 

U  765432    =    U76543C23  -  V76543S23 

*  765432    =    C2V    U76543S23    +     V^^C^  )  -  S2W76543 

W765432  =  s2(  U76543s23  +  V7d543c23  )  +  c2'W-l6543 

^  76543    =    U7654C34  -   V7654S34 

V76543    =    C3(  U^S^    +    V^C*  )   -  S3W7d54 

W76543  =  s3(  U^s^  +  V^c^  )  +  c3W7654 

U7654  =  U7d5c45  -  V765s45,   V7654  =  c4(  U763s45  +  V765c45  )  -  s4W765 

W7654  =  s4(  U765s45  +  V765c45 )  +  c4W765 

U765  =  U76c56  -  V76s56,      V765  =  c5(  U76s56  +  V76c56 )  -  s5W76 

W765  =  s5(  U76s56  +  V76c56 )  +  c5W76 

U76  =  s7s67,  V76  =  -  (  s6c7  +  c6s7c67 ) 

"76    =    V7  "  ^6S7C67 

Substituting  equation  set  (C.2)  through  (C.12)  with  the  actual  SSRMS  mechanism 
parameters  from  Table  4-1  into  eq.  (C.l),  the  position  vector  in  the  first  coordinate 
system,  Rp(1)  is  obtained  as 

RP(1)  =  [RP*(,)>  WT  (C13) 

where 
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*V     =  a23°2  "  S3s2  +  a^c^  -  S4s2  +  a45c2c3+4  -  S5s2  +  a56c2c3+4+5  +  S6C2S3+4+5  + 

^>7(S6C3  +  4  +  5C2    +    C6S2)    +    378("S2S6C7    +    C2(S7S3  +  4  +  5    +    C6C7C3  +  4+5)   ) 
l^py  V2    +    a34S3    +    a45S3  +  4    +    a56S3+4+5  "  ^6C3  +  4+5    +    S7S6S3+4  +  5    +    a78(-S7C3+4+5    + 

C6C7S3  +  4+5.) 

*V     =  a23s2 "  S3C2  +  a^s^  +  S4C2  +  a45s2c3+4  +  SjCj  +  a56s2c3+4+5  +  S6s2s3+4+5 
+  5>7(s6c3+4+5s2  -  c^)  +  a7g(C2S6c7  +  s2(s7s3+4+5  +  CgC^.^.,^) ) 
From  the  equation  (C.12)  the  orientation  vector  in  the  first  coordinate  system,  a78(1) 
can  be  rewritten  by 

*."    =    [V^WT  (C.14) 

where 

a78x         =    "S2S6C7    "*"    C2VS7S3+4+5    +    C6C7C3  +  4  +  5) 

a78y        =    "S7C3+4+5    "•"    C6C7S3+4+5 

a78z         =    C2S6C7    +    S2(S7S3  +  4  +  5    +    C6C7C3+4+s) 

Similarly,  from  the  set  1  in  the  Table  4-2  the  orientation  vector,  Sg(1)  can  be  written 
by 

^8        =    L-^765432)    *765432>  Z765432]  (C.15) 

S  (1)  =  X  S  (1)  =  Y  S  W  =  7 

°8x  yv765432>  °8y  *  765432*  °8z  /j765432 

-^■765432    =    -^"76543C2  "    *76543S2 

*  765432    =    C12V-^-76543S2    "*"     *76543C2.)   "  S12^76543 
^765432    =    S12VA76543S2    "*"      *76543C2)    +    C12^76543 

^■76543    =    X7d54C3  -  Y7654S3>      *  76543    =    C23("*-7654S3    "*"     *7654C3/   "  S23^7654 
^76543    =    S23(-^-7654S3    "*"     ^7654^)    "*"    C23^7654 
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X7654    ~~    X765C4  "  Y765S4,  I  7654    -    C34(X765S4    +     *  765C4J   "  S34if-765 

Z>7654    =    S34(^-765S4    +    Y765C4)    +    C^Z^ 

X765  =  X76c5  -  Y76s5,         Y765  =  ^(X^  +  Y76c5)  -  s45Z76 

Z765    =    S4j(X76S5    +      I76C5)    +    CA5Z16 

X7<  =  X7  c6  -  Y7  s6,       Y76  =  c56(X7  s6  +  Y7  c6)  -  s^Z, 

Z76  =  s56(X7  s6  +  Y7  c6)  +  c56Z7 

X7    =  s78s7,  Y7    =  -  (  s67c7g  +  c67s7gC7 ) 

^7        =    Cffin  "  S67S78C7 

Substituting  equation  set  (C.16)  into  eq.  (C.15)  gives 
c  (i)  _  re  (i)  c  (i)  c  (i)iT 

^8        —    L°8x     >°8y     >°8z     J 

where 


(C.16) 


^8x         =      C2VS7C6C3  +  4  +  5  "  C7S3  +  4  +  5.)   "  S2S7S6 

^8y         =      S7C6S3+4  +  5    "*"    C7C3+4+5 

$8z        =     S2VS7C6C3+4+5  "  C7S3+4+5/    "*"    P^A 

Position  and  orientation  vectors  known  in  terms  of  the  first  coordinate  system  can 
be  found  in  terms  of  the  fixed  coordinate  system  as  follows: 

Rp  =  [M1IRW],       a7g  =  [M][a78<^],       S,  =  [M][S«]  (C17) 

where 


M  = 


COS0j 

-sin0x 

0 

sin01 

COS0! 

0 

0 

0 

1 

APPENDIX  D 
DERIVATION  OF  JACOBIAN  MATRIX  OF  7R  MANIPULATOR 


The  Pliicker  coordinates  of  each  of  the  seven  joint  axes  are  comprised  of  the 
direction  of  the  joint  axes  and  the  moment  of  that  line  about  the  origin  of  the 
coordinate  system.  The  directions  of  each  joint  axis  measured  with  respect  to  the 
current  coordinate  system  are  listed  directly  as  set  8  in  Table  4-2.  It  has  also  been 
shown  in  Duffy  [12]  that  the  moment  of  each  line  about  the  origin  is  equal  to  the 
dual  of  the  direction  vector.  The  Pliicker  axis  coordinates  of  the  six  axis  lines, 
ii  =  [S0i;Si]T,  are  listed  as  follows: 

$i(8)  =  [(-a78a7g<8)  -  S7S7<8>  -  S&w  -  W>  -  S&®  -  W»  -  S£«>  -  a^8> 

-  s&m  -  W  -  $&"*&"  ;  &w  ]T  (D.l) 
i2(8)  =  [(-a78a78<8>  -  S7S7<8>  -  S£*>  -  a**,®  -  S&m  -  a*A,«  -  S^  -  a^ 

-  S&m  -  WW>  ;  S2<8>  r 

i3(8)  -  [(-a78a78<8>  -  S7S7<8>  -  S&P  -  a56a56(8)  -  S£*>  -  a45a/>  -  S^  -  a^'8' 

)xS3(8) ;  &•  ]T 
1(8)  -  [(-a78a78(8)  -  S7S7<8>  -  S&«>  -  a**,*)  -  S&?>  -  a^)*^  ;  fi,"  ]T 
&(8)  -  [(-a78a78<8>  -  S7S7<8>  -  S&m  -  WOx^  .  s5<8>  ]T 
&(8)  =  [(- W°  -  S&m^m  ;  &«>  f 
i7(8)  =  [-«■•  x  S/»  ;  S,"  ]T 
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The  Pliicker  line  coordinates  are  derived  as  follows: 

&«      =  [0,  -s81,  c81]  (D.2) 

=  [0,  0,  1] 
&■  -  ft  ,  Y,  ,  Z,  ]  (D.3) 

Ax      =  S^Sj,  Y5      =  -  (  SglC!2  +  CgjS^Cj  ) 

&»  =  [X21,  Y21,  Z21]  (D.4) 

A21  =  A2  Cj  -  Y2  slt      Y21  =  cgl(A2  Sj  +  Y2  Cj)  -  s81Z2 

^21    =    S8l(X2    Sl    +     *2     Cl)    +    C81^2 

-^■2      =  S23S2,  Y2      =  -  (  S^C^  +   C^SjjCj  ) 

z^    —  c^c^  -  s^s^^ 
S^  -  [X321,  Y321,  Z321]  (D.5) 

A^j  =  A32C!  -  Y32Sj,    Y321  =  Cgj^A^Si  +   Y32Cj)  -  SglZ,32 

•^321    =    S8l(X32S!    +      I32CJ    +    C8lZ32 

X32  =  X3  Cj  -  Y3  s2,  Y32  =  c12(X3  s2  +  Y3  C2)  -  SJ2Z3 

■^32    =    SU\X3S2   +    Y^)    +    CUZ3 

X-3        =    S34S3,       Y3        =    -(S23C;}4    +    C23S34C3) 

"i        ~    ^23^34  "  S23S34C3 

&«  =  [X4321,  Y4321)  Z4321]  (D.6) 

X|321    =    Xj32Cl  "    *432S1>  *4321    =    C8l(X432Sx    +     ^432Cl)   "  S81^432 

•^4321    =    S8l(-^-432Sl    "*"     M32Cl)    "*"    C81^432 

A-432    =    ^-43^  "    *43S2>      *432    =    C12(X43S2    +     Y43C2)   -  S^Z,^ 


J43 
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A32    =    S12V^43S2    +     * 43C2)    +    C12^43 

X43    =   A4    C3  -   Y4    S3,  Y43    =    C23(X4    S3    +    Y4    C3)  -  S23Z4 

Z43  =  S23(X4S3  +  Y4c3)  +  C23Z4 

X4        =    S^,       Y4        =    "(Sj^^    +    C34S45C4) 

Z»       =    C34C45  "  S34S45C4 
^6         =    I.X54321,      Y5432J,      Z5432J  (D.7) 

•^■54321    ~    "^5432C1  "    ^5432S1>  *  54321    =    C8l(X.5432S1    +     Y5432CJ)  -  SsxZ^j 

■^54321    =    S8l(X.5432S1    +    Y5432CJ    +    08^5432 

X5432    =    X543C2  "    1 543S2,  Y 5432    =    C^X^Sj    +      154302)   -  S^Zj^ 

^5432    =    S12(X543S2    +    Y^jCj)    +    C^Z^j 

X543    =    X54C3  -   Y54S3,     Y543    =    C23(X54S3    +    Y54C3)  -  S23Z54 
Z543    =    S23(X54S3    +    Yj^jCj)    +    C23Z54 

X54  =  X5  c4  -  Y5  s4,  Y54  =  C34(X5  s4  +  Y5  c4)  -  S34Z5 

Z54    =    S34(X5S4   +    YjC4)    +    C34Z5 

X5     =  s56s5,     Y5     =  -(s45c56  +  C45S56C5) 

^5        =    C45C56  "  S45S56C5 
^7        =    1X654321,      I«54321>     ^-(Mi2U  (D.8) 

-^■654321    =    X65432CJ  -    Y 554328!,  Y5543J!    =    08^X554328!    +    Y^jjCj)   -  Sg1Z65432 

■^654321    =    S8l(X65432S1    +    Y55432CJ    +    €8^55432 

X«5432    =    Xj^Cj  -    Y6543S2,  Y55432    =    Ci2(X6543S2    +    Y5543C2)   -  8^55432 

•^■65432    =    S12(X6543S2    +    Y5543C2)    +    CJ2Z5J43 
-^■6543    =    X5J4C3  -    Y  55483,  I  6543    =    C23(X654S3    +    ^55403)   -  S23Z554 


J65 


"6 
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A543    =    S23(X&54S3   +    Yj^Cj)    +    CjjZg^ 

-^654    =    ^65C4  "  Y^,    Y^    =    C34(X65S4   +    Y^Q,)  -  S^Z, 

■^"654    =    S34(^«5S4    +    Y^C,,)    +    C^Z^ 

X«  =  Xfi  c5  -  Y6  s5,  Y^  =  cJXo  s5  +  Y6  c5)  -  s^Z, 

Zss  =  sJX.Ss  +  Y6c5)  +  c45Z6 

X*    =  s67s6»    Y6    =  -(s56c67  +  c56s67c6) 

**        =    C56C67  "  S56S67C6 

a^(8)  =  [q,  Ulg\  UJ  (D.9) 

a23(8)  =  [W21,  -U218*,  U218]  (D.10) 

"21  =  cic2  "  SiSjCjj,     U218  .=  U21sgl  +  V21cgl 

U218  =  U21c81  -  V21sgl, 

U21  =  s^,  V21  =  -  (  s^  +  CjSjC^  ) 

as4(8)  =  [W321,  -U3218*,  U3218]  (D.ll) 

w32i  =  s1(U32s12  +  V32cn)  +  CjW32  U321g*  =  U321sgl  +  V321c81 

^3218    =    U321Cgl  -    V321Sgl, 

U321   =   V32CU  -  V32S12,  Vm   =   SiOJjjSc   +   V^)   +    qW^ 

W321  =  s^U^  +  V^)  +  qW,, 

U32  =  s^,  V32  =  -  (  s2c3  +  C2S3c23 ) 

"32    =    ^-2^  "  ^SsC^ 

a45(8)  =  [W4321,  -U4321g\  U4321g]  (D.12) 


-81 


'432 
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W432i  =  s^U^Su  +  V432cn)  +  c,W432        U43218*  =  U4321s81  +  V4321cg 

U43218  =  U4321c81  -  V4321s81, 

U4321  =  ^^c^  -  V432su,         V4321  =  s1(U432s12  +  V432c12)  +  qW* 

U432  ■  U43C23  -  V43s23,  V432  =  SjCU,^  +  V^Ca)  +  c,W43 

W432  =  s^U^  +  V43C23)  +  c2W32 

U»3    =    S^^,  V43    =    -   (  S3C4    +    C3S4CJ4  ) 

"43    =    C3C4  "  S3S4C34 
a56         =    ["S4JH,   -U543218  ,  Uj^jg]  (D.13) 

^54321    =    SlCU^jS^    +    Vj^C^)    +    CjW^j 
^543218      =    *-'54321S81    +    V54321C81 


'5432 


^543218    -    U^^jCgj  -  V54321Sg1, 


^54321    =    UJ432CJ2  "  V^jSjj,         VM321    =    S^U^S^    +    V^C^)    +    CjW, 

U5432    =    U^C^  -  V543S23,  V^    =    82(11543823    +    V^j)    +    C2W543 

WM32    =    52(^3823    +    V^C^)    +    C2W532 

U543    ■    U54C34  "  V54SJ4,  V543    =    83(^834    +    ¥54034)    +    C3W54 

W543    "    S3(U54S34    +    V54C34)    +    C3W54 

U54    =    S5S45,  V^    =    -   (  S4C5    +    C4S5C45  ) 

*^54    =    C4C5  '  S4S5C45 
E67         =    [WJ5432J,   -Ubchj  ,  U^^jg]  (D.14) 

^65432!    =    S1(U65432S12    +    Vg^C^)    +    CjV/is432 
'-'6543218      =    U654321Sg1    +    VS^jjCg! 
^6543218    =    U654321C81  "  V 654321S81> 
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^654321    =     U55432CJ2  -    V^s^S^, 

» 654321    =    S1(U65432S12    +    V^j^C^)    +    0^65432 

^65432    =    UJ543C23  -  V^jSja,  V65432    =    S2(Ud543S23    +     V  4543023)    +    CjW^j 

"65432    =    S2\*-'6543S23    "*"     *^6543C23)    "*"    ^"6532 
^6543    =    Ug^C^  -  V654S34,  V^j    =    83(1)454834    +    V^Cjj)    +    C3W6S4 

"6543    =    S3(L'654S34    +    ^654C34)    +    C3"654 

UfiM    =    U^C*  -  V^j,  V^    =    54(065845    +    VMC«s)    +    C4W65 

W^    =    54(1^45    +    V^C*)    +    C4W65 

U&5  =  s6s56,  Vb  *  -  (  s5c6  +  c5s6c56 ) 

"65    =    PA  "  ^5^6^56 
a78         =    ["7654321*  "U76543218  »    *-J765432j8J  {U.IJ) 

"7654321    =    Sl(U765432S12    +      *765432C12)    +    Cl"  765432 
^76543218      =     ^7654321S81    "*"      *7654321C81 
^76543218    =    U7654321Cgl  -    V765432iSgl, 
U  7654321    =    U765432CJ2  "    V765432S12, 

» 7654321    =    Sl(LI765432Si2    +     V765432C12)    +    CJVV745432 
^765432    =     LI76543C23  -    V76543S23, 

*765432    =    S2V*-'76543S23    +     *76543C23)    +    C2"76543 
"765432    =    S2(L'76543S23    **"      *76543C23)    "*"    C2"  76532 
U76543    =    U7654C34  -    ^7^34,  ^74543    =    S3(U7654S34    +    V-;,;^^)    +    C3W7654 


W76543    =    S3(U7654S34   +    V^C^)    +    C3W7654 

U7454    =    U745C45  -  V765S45,  V76S4    =    S4(U765S45    +    V765C45)    +    C4W765 


106 
W7654  =  s4(U765s<5  +  V^cJ  +  c4W765 

U765  =  U76c56  -  V^,  V765  =  s5(U76s56  +  V76c56)  +  c5W76 

W765  =  s5(U76s56  +  V76c56)  +  c5W76 
U76  =  s7s67,  V76  =  -  ( SjC,  +  c6StC67  ) 

"76    =    C6C7"  S6S7C67 

&•*&«     =       [-Cl,  -Sl,  0]  (D.16) 

&23     XOj  =       [SjC2,      "C1C2,     UJ 

S,^®     =       [-Sls2)  Cls2,  0] 

a^Wxgw    =    [(SlC2c3  +  q%),  -(CjCjC,  -  s^),  0] 

S^xS^     =       [-sa,  Cls2,  0] 

au^Si®     =     [(s^C*  +   0,534),  -(CxCjCa*  -  S^)  ,    0] 

S^xS^     =       Ks2,  Cls2,  0] 

W^Si®     =     [(SlC2C345  +   CjSsc),    -fac^  -  s1s345),    0] 

Sj^xSi®        =  [foC^  -  qC*,),     -faC^    +    SjC^),     0] 

S7(8)xS!(8)      =  [(s^Cj^Cj  +  S!C6S2  +  CjS^),  -(CiSfiC^jCj  +  CXC6S2  -  SjSeS^),    0] 

a^xS/8*    -    [-(s1s2s6c7  -  s^CstSmj  +  c^c^)  +  c^StC^  -  c^s^)),   fos^c, 

+  CjCjCStS^  +  c^Cms)  -  s1(s7c345  -  C^Sa^)),  0] 
a^xS^    =    [Cls2,  Sls2,  -c2]  (D.17) 

03  X02  =  [CiC2,    SjC^    S2J 
§-34     X^2          =      L^lS^*     SjS^,     -C2C3J 

04  X02  =  LC1C2»     ^1^'      ^2J 

S-45     X^2  =       lClS2C34'      S1S2C34>      _C2C34j 


107 


o     (8)YC  (8)       _ 


o     (8)YC  (8) 


S4(8)XS3(8) 


S5(8)XS3(8)        = 
„     (8)    O  (8)       = 

S7(8)xS3<8)     = 


a    (8)YQ  (8) 
d78    A03 


a    <8>yS  (8) 


&•*,«■ 


a     (8)YQ  (8)       _ 
S78     A^4 


LC1S2C345>     S1S2C345»     _C2C345J 
lClS2S345>     SlS>2S345»      -C2S345J 

[(CiS6C345S2  +  CjCjCj),  -(S1S6C345S2  "  S!^^),  -(SjCjcCj  +  c6s2)  ] 
[(c^SsC,    +    CaCStS^    +    Cfifi^)),      (SjCjS^    +    s1c2(s1S3«    + 

C6C7C345)X   (S2S6C7  -  CjCS^^j    +    C^Cj^))] 

[(CiC2s3  +  StCj),   (SjCjSa  +  c^),   s2s3]  (D.18) 

[0,  0,  0] 

[(Cl^M    +    S1C34)>   (Sl^M  "  ClCy*)f     S2S34J 

[0,   0,   0] 

[(C^S^j  +  SjCjtf),  (S1C2S345  +  C1C345),  S2C345J 
[-(CJCJC345  -  S1S345),  "(SjCjCj^  +  0,8345),  S2C345J 
[(ClV^^    +    S1C6C345)>    (Sl^MS^  "  C1S6C345)>      S6S345S2J 

[-(CiC2(s7C345  -  c6c7S345)  +  Sxi&ihv  +   ^c^s)).     (-SAisftv  - 

C6C7S345)   "  Cl(S7S345    "*"    C6C7C345))>  "S2VS7CJ45  "   C6C7S345jJ 
[(Cl^M    +    S1C34)>   (S1C2S34  "  C1C34)>      S2S34J 

[0,   0,   0] 

[(CjC^s    +    S1C345),      (SJC2S345    +    C1C345),      S2C345J 

l"(ClC2C345  "  SlS34s)>      "(S1C2C345    +    0,8345),      S2C345J 

l(ClS6S345C2    +    S1C6C345)»   (S1S6S345C2  "  C1S6C345)>      S6S345S2J 
[-(0,02(870345    -    CtCfiya)     +     S,(S7S345     +     C6C7C345)),        (-8,02(8783 

^^Ms)   "   Cl(S7S345    "*"    C6C7C34s))>   "S2VS7C345  "  ^6^3*5)1 


(D.19) 


3345 
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a56(8)xS5(8)       =       [foC^    +    SxO.      ^1<^M5    +    ^CjJ,      S^]  (D.20) 

S6(8)XS5(8)        =  [-(qCjC^  -  SAu),     "(^Cjc    +    0,8*,),      S.Cj^] 

S7(8)XS5(8)      =         [(CjSjS^q  +  SxCjC^j),  (s1s6s345c2  -  qScCba),    S6Sj45S2] 
a7g(8)xS5(8)    =    [icfi&fyu  -  c&fiMs)  +  s,(s7S345  +   cfifiya)),     (-Sfifaha  - 

C6C7S345/   "  Cl(S7S345    +    C6C7C34s))>   "S2(S7C345  "   C6C7S345)J 

S/^xS/*      =  KfeCjuCj    +    S6S2)    +    S^S^,      -SxCPsCmjC,    +    S6S2)  -  CiCjS^j, 

C6C345S2    "*"    $6%  1 

a78(8)xS6(8)    =    [c^s^c,   +   c^c^s)  -  s1s6c7s345,     s^s^c,   +   c^c^)   + 

ClS6C7S345,   "C^CjCy    +    S2S6C7C345J 
a78(8)xS7(8)      =      [-qWl  "   ClC2(C7S345    +    C6S7C345)   "   SiC^C^j    +    C6S7S345)»   -SiS2s6s7  - 
S1C2(C7S345     +     C6S7C34s)     +     ^(CjCju     +     C6S7S345)»    C2S6S7    "    S2(C7S345 

The  Pliicker  coordinates  for  each  of  the  seven  lines  in  equation  (D.l)  can  be 
simplified  by  substituting  equation  set  (D.2)  through  (D.20)  with  the  actual  SSRMS 
mechanism  parameters  from  Table  4-1  into  the  eq.  (D.l).  The  simplified  Pliicker 
axis  line  coordinates  are  as  follows: 

Si  =  L  S0ix      Oly  \lz  »  ^lx  ^ly  ^lzJ  {'-'•*'*■) 

$2  =  I  \lx  ^02y  ^02z  I  ^2x  ^2y  ^2zJ 

5>3  =  [  \ix  ^03y  ^03z  '■>  ^3x  ^3y  ^3zJ 

»L»  =  I  ^04x  ^04y  ^04z  »  \x  \y  \zJ 

£5      =     [  So5X  S05y  S05z  ;  o5x  o5y  o5zJ 

^         =       L    ^06x  ^06y  ^06z  '■>  \x  ^6y  ^6zJ 
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$7         =       I    S07x  S07y  \lz   >  S7x  ^7y  S7zJ 

S0u  =  {'fyfifo  +  S2Ci  +  S3s1s2  -  a34(s1c2c3  +  c^)  +  S4SiS2  -  a45(s1c2c34  +  c^)  +  SjS^  - 

&36\*ApMS    "*"    ClS34s)     +     S6(C1C34j    -    SjC^j^)    -    S7(S1S6C345C2     +     SjC^Sj    +     CjSgS^)     + 
a78VSlS2S6C7  "  Sl^C^MS    +    C6C7C345/    +    C1S7C345  "  ClC6C7S34s))/ 
^Oly    =     {fyfifi?.    ***&  '  Ss^Sj    +    334(C1C2C3  "   S1S3)   "   S4CA    +    a45\ClC2C34   "   S1S34)   "  ^5C1S2    + 
a56VClC2C345  "  S1S345)    +    SjfoC^    +    C1C2S345)    +    S7(C1S6C345C2    +    CjC6S2  -  SJSJS34J)    +    a7g(- 

c1s2s6c7  +  CiC2(s7s345  +  c6c7C345)  +  s1s7c345  -  s1c6c7s345;)} 
Sou  =  "»       Su  =  0,       Sly  =  0,       S^  =  1, 
So2x  =  \"a23cis2  "  ^c^Cj  -  a34c1s2c3  -  S4C!C2  -  a45c1s2c34  -  o^c^  -  aJ6c1s2c345  -  i>6c1s2s345 

S7(c1s6c345s2  -  CjCjCj)  -  a7g(c1c2s6c7  +  c1s2(s7s345  +  Cfifog))  } 
So2y  =  \"a23sis2  "  SsSjC^  -  a34s1s2c3  -  S4s1c2  -  a45s1s2c34  -  ojS^  -  a56s1s2c345  -  o6s1s2s345 

S7(s1s6c345s2  -  sxc6c2)  -  a7g(s1c2s6c7  +  s1s2(s7s345  +  c6CjCM5))  ) 
So2z  =  xPrfii  "  S3S2  +  %■■!&£% "  S4s2  +  34502^  -  S5s2  +  £56^0345  +  SjC^s^j  +  o7(s6c34Sc2  + 

c6s2)  +  a78(-s2s6C7  +  c^s^j  +  pgCjCi^))  } 
Six  =  su       S^  =  -Cj,       Ojj.  =  U, 

So3x  =  v*s«(*Va8i  +  SiCj)  -  a45(c1c2s34  +  SjC^)  -  a56(c1s2s345  +  s1c34S)  +  o6(c1c2C345  -  s^^)  - 
S7(c1s6S345C2  +  s1s6c345)  +  a7g(c1c2(s7c345  -  Cfifoxj  -  s1(s7s345  +  c^c^))} 

So3y    =      l"a34(SlC2S3  "  C1C3)  "  a4s(SlC2S34  "  C1C34/  "  a56VClS2S345  "  C1C345)    +    S6(S1C2C345    +    Cj^s)  - 

S7(s1s6s345c2  -  CjSjCj^)  +  a7g(s1c2(s7c34J  -  c^Sy^)  +  c^s^s^  +  05070345))} 
S03z  =  {-a34S2s3  -  a45s2S34  -  a^s^  +  S6s2C345  -  S7s6S345s2)  +  a7gs2(s7C345  -  Cfifogj)  } 
o3x  =  -(^Sj,        o3y  =  -SjS2,        o3z  =  c^, 
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SMx  ■  {-a^qc^  +  s^)  -  a56(c1c2s345  +  s^)  +  S^qc^  -  s1s3«)  -  S^qs^Cj  + 

S1S6C345)    +    ^l&^l^K^MS  '  ^^Ms)  "  Sl(S7S345    +    C6C7C345/)/ 

SMy  =  {-a^c^  +  qc-a)  -  aJjiiCfrv  +  c^)  +  S^s^c^  +  c^)  -  S7(s1sss34SC2- 
C1S6C345)  +  a78(s1c2(s7c345  -  c6c7S345)  +  c1(s7s345  +  c^c^))} 

S(Mz    =     ("a45S2S34  "  a56S2S345    +    S6°2C345  -  S7S6S345S2    +    a^^Cj^  -  €4078345)} 

o4x  =  -CjS2,  o4y  =  -SXS2,  o4z  =  C2, 

S05x  =  {-a56(CiC2s345  +  SjCms)  +  S^qcjc^  -  s^)  -  S7(c1s6s345c2  +  s^c^)  +  a78(c1c2(s7c345 

-  C6C7S345)  -  s1(s7s345  +  C6C7C345))} 
Sosy  -  {-&s&&h«  +  cic345)  +  S^s^c^s  +  qs^)  -  S7(s1s6Sj45c2-c1s6c345)  +  a78(s1c2(s7c345  - 

C6C7S345)    +    Cl(S7S345    +    C6C7C34s))/ 
So5z    =     {"a56S2S345    +    §&$MS  "  S7S6S345S2    +    a78S2(S7C345  -   CjCyS^)} 
^5x    =    ~C1S2>  ^5y    =    "S1S2>  ^5z    =    ^2» 

So6x  "  {S7(c1c6c345c2  -  CjS^fc  -  s&Sms)  -  a78(c1s2c6c7  +  CjC^fifi^  -  s1s6c7s345)} 
So6y  =  {S7(s1c6c345c2  -  s1s6s2  +  qc^s)  -  a78(s1s2c6c7  +  s1c2s6c7c345  +  c^c^)} 

So6z    =     {S?(C6C345S2    +    S6C2/    +    a78(C2C6C7  -  8284070345)} 

$6x   =    C1C2S345    +    S1C345)  $6y    =    SjC^s  -  CjC^j,  C>6z    =    S2S345, 

S()7x    =    a78VClS2S6S7    +    C1C2\C7S345  "  C6S7CJ«)    +    Sl(C7C345    +    C6S7S345)) 

So7y  =  a78(s1s2s6s7  +  s1c2(c7S345  -  c6s7C345)  -  c1(c7c345  +  C6S7S345)) 

^07z  =    a78(-c2s6s7  +  S2(C7S345  -  C6S7C345)) 

^7x    =    C1C2S6CJ45    +    C1S2C6  "  S1S6S345>  ^^    =    SJC2S6CJ45    +    S^Cg    +    C^S^j, 

^7z    =    S2S6C345  "  C2C6> 


APPENDIX  E 
DERIVATION  OF  DERIVATIVE  JACOBIAN  MATRIX  OF  7R  MANIPULATOR 


[SJ] 


T    _ 


«JU 

<5J21 

6J31 

6J41 

SJ51 

*J« 

«SJn 

5J22 

5J32 

SJ42 

*J52 

«J«2 

6J13 

5J23 

5J33 

*J43 

5J53 

«Jfl 

5J14 

«J24 

«Jm 

5J44 

5J54 

*J« 

5Jl5 

6J25 

5J35 

5J45 

5J55 

<5J« 

5Jl6 

5J26 

*J* 

SJ46 

«J56 

5J66 

6J17 

6J27 

<5J37 

SJ47 

<5J57 

«5J67 

SJ^  =  Up.,60,  +  Vpq6e2  +  Wpqse3  +  uvM«e4  +  vwR$e5  +  uwN5e6  +  uvw^e,, 

p  =  1,  2,  ...,  6,  q  =  1  2,  ...,  7. 

uu  =    -S2sx  -  a^qq  +  S3CjS2  -  a34(c1c2c3  -  SjO  +  S^Sj  -  a^qqc^  -  s^)  +  SsC^  - 

assCCiCzC^  -  s^)  -  S^qC^^  +  s^)  -  S7(c1(s6c345c2  +  c6s2)  -  S^S^)  + 

a78(c1s2s6c7  -  c1c2(s7s345  +  q^c^)  -s^e^  -  c^s^)), 
u21  =    S2q  -  a23SiC2  +  S3SjS2  -  a^s^  +  eft)  +  S4SJS2  -  a45(s1c2c34  +  c^)  +  S^^  - 

a^SiC^  +  qs^)  -  S6(s1c2s2,3+4  -  qc^)  -  S7(s1(s6c345c2  +  c6s2)  +  qs^)  + 

a78(s!S2s6c7  -  s1c2(s7s345  +  CjCjC^)  +  c1(s7c345  -  c^s^)), 
U31  =  u41  =  u51  =  u61  =  0.0, 
uu  =    a23S!S2  +  S^^  +  a34s1s2c3  +  S^q  +  a45s1s2c34  +  S^^  +  as^s^  +  S6s1s2s345 

+  S7(s1s6c345s2  -  SjC^)  +  a^s^s^  +  s1s2(s7s345  +  qqc^)), 


HI 


"36 
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1*22  =    -^23cih  -  S3CjC2  -  a^CjSjCj  -  S^Cj  -  a^CjSjC^  -  SjC^  -  a^c^c^  -  S&SjSm,  - 

S7(c1s6C345S2  -  qc^)  -  a^qc^q  +  c^s,^  +  qc^)), 
u32  =  0,       u42  =  0,       u52  =  0,       u^  =  0, 
ui3  =    a34(s1c2S3  -  c^j)  +  a45(s1c2S34  -  c^)  +  a^Sjqs^  -  c^)  +  S^s^c^  -  cfaj  + 

S7(s1s6s345c2  -  c^c*,)  -  a78(s1c2(s7c345  -  c^s^)  -  q^s-^  +  c^c^)), 
u23  =    -a34(c1c2s3  +  s^j)  -  a45(c1c2s34  +  s^)  -  a56(c1c2s345  +  s1c34S)  +  S^qc^  -  s^)  - 

StCciSsS^^   +  SiSjC^)  +  a78(c1c2(s7c345  -  c^s^)  -  s1(s7s345  +  qqO), 
u33  =  0,       u43  =  0,       u53  =  0,       u63  =  0, 
uM  =    a45(s1c2s34  -  qqj  +  a56(s1c2s345  -  qqj  -  S6(s1c2c345  +  c1s345)  +  S7(s1s6s345c2    - 

CiSsC^j)  -  a^s^s^  -  0^8345)  -  c1(s7s345  +  qqO), 
u24  =    -  a45(c1c2s34  +  SjCj,)  -  a^qc^j  +  s^)  +  S6(c1c2C345  -  s^)  -  S7(c1s6s345c2    + 

S1S6C345)  +  a78(c1c2(s7c345  -  c6c-,sM5)  -  s1(s7s345  +  qqO), 
U34  =  0,       uM  =  0,       uM  =  0,       uM  =  0, 
Uu  =    a56(s1c2s345  -  qq^)  -  S6(s1c2c345  +  qSj^)  +  S7(s1s6s345c2  +  qs^)  -  a78(s1c2(s7c345  - 

C6C7S345)   "  Cl(S7^345    "*"    C6C7C345^» 
U25    =      "     ^(qCjS^      +      SjCys)      +      S6(C1C2C345     -     S^jJ      -     S7(C1S6S345C2  +      SiSjC^j)      + 

a78(ClC2\S7C345  "   C6C7S34s)   "  Sl(S7S345    "*"    C6C7C345)J> 
U35    =    0,  U45    =    0,  U55    =    0,  Utf    =    0, 

Uw  =     S7(S1(-C6C345C2  -  S6S2)   +   C1S6S345)   +   ^(^(SjCjC,  +   C^C^)   +   C1S6C7S345), 

u26  ■    S7(c1(c6c345c2  -  s6s2)  -  SjS^)  +  a78(-q(s2qq  +  W^c^)  +  s1s6qs345), 
U36  ■   0, 

U46    =      "S1C2S345    "*"    C1C345' 
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U56    =      C1C2S345    +    S1C345> 

U«  =     0, 

u17  =    a78(c1(c7c345  +  c^s^)  +  ^(c&M*,  -  c2^ms  -  s2s6s7)), 
u27  =    a78(s1(c7c34S  +  c6s7Sj45)  +  c^-c^StC^  +  CjC^  +  s2s6s7)), 

u37  =  0, 

u47  =  -s1(c2s6c345  +  s2c6)  -  c1s6s345, 

U57    =    Cl(C2S6C345    +    S2C6)  "  S1S6S345' 

u67  =  0, 

vu  =    a^h  +  S3s1c2  +  ajjS^  +  S^jC^  +  a45s1s2c34  +  S^^  +  a56s1s2c345  +  S6s1s2s345 

+  S7(s1s6c345s2  -  SiCjCj)  +  a78(s1c2s6c7  +  s1s2(s7sJ45  +  CgC^)), 
v21  =    -a^qs;,  -  SjC^  -  a34c1s2C3  -  S,,^  -  a4jC1s2C34  -  Sjqc,  -  a56c1s2C345  -  860^25345  +  S7(- 

CiS6C34Ss2  +  C!C6c2)  -  a78(c1c2s6c7  +  c1s2(s7S345  +  c^c^)), 
V31  =  v41  =  v51  =  v61  =  0.0, 
\n  =    -a^CjC^  +  SjC^  -  a^CjCjCj  +  S4c1s2  -  a45c1c2C34  +  SjC^  -  asgC^c^  -  S^^s^  + 

S7(-CiS6C345C2  -  qc^)  +  a78(c1s2s6c7  -  (^(s^  +  c^Cj^)), 
v^  =    ^s^  +  838^2  -  a34s1c2c3  +  S4S!S2  -  a45S1c2c34  +  S5s1s2  -  aj^CjCj^  -  S6s1c2S345  + 

S7(-s1s6C345c2  -  s^)  +  a^s^s^  -  s1c2(s7Sj45  +  c6c7C345)), 
v32  =    -a^  -  S3C2  -  aj4S2c3  -  S4c2  -  a45S2C34  -  S5c2  -  a56s2C345  -  S^Sj^  +  S7(-s6C345s2  +  c6Cj) 

"*"    a78("C2S6C7  "  S2VS7S345    "*"    C6C7C34s))> 
V42    =    V52    =    V62    =    0.0, 

vn  =    aj4c1s2S3  +  a45C1s2S34  +  a56c1s2S345  -  S^^c^  +  S7c1s6s345s2  -  a78c1s2(s7c345  -  c^c^)), 

V23    =       a34SlS2S3   +    a45SlS2S34   +    a56SlS2S345  "  ^SjS^j^    +    S7S1S6S345S2     "  a^S^^-^s  -  06078345)), 
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V33  =      "^W^A  '  a45C2S34  "  a56C2S345    +    S^C^  -  S^S^Cj      +    ^7802(870345  -  Cfifagj), 

v43  -  -C1C2.       v53  =  -S^,       v63  =  -s2, 

V14  =       a45ClS2S34    "*"    a56ClS2S345  "  S6C1S2C345    +    S7C1S6S345S2     "  a78ClS2(S7C345  "   C^S^)), 

V24  =       a45S1S2S34    +    a56S1S2S345  -  S6S1S2C345    +    S7SiS6S345S2     -   a78S1S2(S7C345  -  €5078345)), 

V34  =       "  a45C2S34  "  a56C2S345    +    Si^MS  '  ^SflSj^Cj      +    a78C2(S7C345  -   €6078345)), 

V44  =  <i<h>       v54  =  -S1C2,       vM  =  -s2, 

V15  =       a56ClS2S345  "  S6CJS2C345    +    S7CJS6SJ45S2     -  a7gC1S2(S7C345  -  C6C7S345)), 

V25  =       a56SlS2S345  "  SiSAQs*    +    S7S1S6S345S2     -  a7gS1S2(S7C345  "  C6C7S345)), 

V35  =       "  a56C2S345    +    S6C2C345  -   8784834502      +    a7gC2(S7C345  "  ^€78345)), 


V45  =  -Cfi*  V55    =    -SiCj,  \a    =    -S2, 

v16  -    -S7(c1(c6C345S2  +  s^))  +  a78(c1(s2s6c7c345  -  Cfifr)), 

v26  -    -S7(s1(c6C345S2  +  s^))  +  a78(s1(s2s6c7C345  -  c2c6c7)), 

V36  =     S7(C6C345C2  -  S6S2))  -  Z-nic^fiiCya  +  S2C6C7)), 

V46  =  "C1S2S345>  V56    =    ~S1S2S345>  V46    =    C2S345» 

V17  =       a78Cl(C2S6S7  "  S2C7S345    +    S2C6S7C345.)> 

V27  =       a78^1VC2S6S7  "  S2C7S345    ~*~    S2C6S7C34s)> 

V37  =       a78VS2S6S7    +    C2(C7S34S  "  C6S7C345))» 

V47  =  "Cl(.S2S6C345  "  C2C6)» 

v57  =  -s1(s2s6c345  -  C2C6), 

V67  =  C2S6C345    +    S2C6» 

wM  =  a34(s1c2s3  -  qcj)  +  a45(s1c2S34  -  c^)  +  a56(s1c2S345  -  c^)  -  S6(s1c2c345  +  c#m)  + 

S7(S1S6S345C2     -  CiS6C345)  -  a7g(S1C2(S7C345   +    C6C7S345)  -  c1(s7s345    +    C6C7C34S)), 
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w21  =  -a^c^Sj  +  s^)  -  a^Cc^s^  +  s^)  -  a^qc^  +  s^)  +  S^c^c^  -  SjSjJ  - 

S7(C1S6S345C2     +    S1S6C345)    +    378(0102(870345  -  C6C7S345)  -  Si(S7S345   +    04070345)), 

w3i  =  w41  =  w51  =  w61  =  0.0, 

W12    =     a34ClS2S3   +   a45ClS2S34   +    a56ClS2S345  "  S4C1S2C345   +    S7CiS6S345S2    ■  a7gC1S2(S7C345  -  04078345)), 

W22    =     a34SlS2S3   +    a45SlS2S34   +    a56SlS2S345  "  S4S1S2C345   +    S7S1S4S345S2    "  a78SlS2(S7C345  "  C6C7S34s))> 

W32    =     "a34C2S3  "  a45C2S34  "  a56C2S345    +    56C2C345  -   S7S6S345C2      +    a7gC2(S7C345  -   04078345)), 

W42    =    0,  W52    =    0,  W42    =    0, 

W13    =      "a34(ClC2C3   "   hh)   '    a4s(ClC2C34  "   S1S34)    "    ^xifiAfinS   '   SlS34s)    "   84(01038345    +    S^j)    - 

87(0^4034502  "  S1S4S345)   -  378(0^2(878345    +    C4C7C345)    +    8^870345  -  C4C7S345)), 
W23    =     -^(SlCjCj    +    0^3)  -  345(8^2034    +    0^34)  -  a54(S1C2C345    +    0^345)  -  Ss^jC^j  -  0^345)  - 

S7(S1S6C345C2    +    0^48345)  -  378(8^(878345    +    C4C7C345)    +    0^-870345    +    04078345)), 
W33    =      "334S2C3  "  345S2C34  "  354S2C345  -  04828345  -  0784034582     "  37582(878345    +    C4C7C345)), 
W43    "    0,  W53    =    0,  W43    -    0, 

W14    =     -  345(0^034  -  S1S34)  -  356(CiC2C345  -  S1S345)  -  S6(C1C2S345   +   SjC„s)  -  S7(C1S4C345C2  -  S1S4S345) 

"  378(C1C2(S7S345    +    C4C7C345)    +    S1(S7C345  -   C4C7S345)), 
W24    =      -    345(8^034    +     C1S34)    -    354(8^0345    +     0^345)    -    S4(S1C2S345    -    0^345)    -    87(8^4034502    + 

C1S6S345)   "  a78(SlC2\S7S345    "*"    C6C7C345)   "   Cl(S7C345  "   C6C7S345))' 
W34   =     "  345S2C34  -  354S2C345  -  S4S2S345  -  S7S4C345S2     "  a78S2VS7S345    +    GfifotSJ/i 
W44    =    0,  W54    =    0,  WM    =    0, 

W15    =     "  a56(ClC2C345  -  S1S345)  -  S6(CiC2S345    +    S1C345)  -  87(0184034502  -  S1S4S345)  -  378(0105(878345    + 

C6C7C34S/   '  Sl(S7C345  "  C6C7S345))» 
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Wzs  =  -  ZseisfoCw  +  C1S345)  -  S^s^s^  -  c^)  -  S7(s1s6C345c2  +  w^j)  -  a7g(s1c2(s7s345 

+    C6C7Cj45)   -  Ci(S7C34j  -  CfaSyis)), 
W35    =      "  a56S2C345  "  S6S2S34S  -  S-^C^jSj     *  a78S2VS7S345    +    WwJl 

w45  =  0,       w55  =  0,       w^  =  0, 

W16    =      "^(ClCsC^jCj    +    SjCgC^s)    +    a78(ClC2S6C7S345    +    SlS6C7C34s)> 

w26  =  "S7(s1c6c345C2  -  c^c^)  +  a78(s1c2s6c7s345  -  CiSjCrCge), 

W36    =      "^7^5345^2    "*"    a78S2^6C7S345' 

W46    =    CjCj^  -  SjS^j,  WJ6    =    SiCjCj^    +    C^,  Wtt    =    SjCju, 

W17  =      a78(ClC2(C7CM5    +    C6S7S345)   "  Sl(C7S345  "   C6S7C345.).)» 

W27  =      fywhp2\Pfyo    +    C6S7SM5)    +    Cl(C7S345  "  C6S7C345.),)> 

W37  =     a78S2(C7C345    +    C6S7S345/> 

W47  =  -Cl<h.^345  -  WmS.  W57    =    -S^SjS^j    +    C1S6C345,  W67    =    -S2S6S345, 

uvn  =     a45(s1c2s34  -  0^34)  +  a^s^s^  -  c^)  -  S^s^c^  +  c^)  +  S7(s1s6Sj45c2   - 

ClS6C34S)  -  378(8102(870345  -  C6C7S345)    +    Cjfah*    +    W^j)), 
UV21    ■  -  345(0^2834   +    S^)  -  a56(C1C2S345    +    S^)    +    S6(C1C2C345  -  S^)  -  S7(C1S6S345C2      + 

SlS6C34s)    +    a78(C1C2(S7C345  -  C6C7S345)   -  S1(S7S345    +    C^Cjjj)), 

uv31  =  0,       uv41  =  SjSj,       uv51  =  -CjSj,       uv61  =  0, 

uvu  =      a4Sc1s2s34  +  a^c^s^  -  S^s^  +  S7c1s6S345s2  -  a7gCiS2(s7C345  -  c6c7Sj45)), 

UV22  =     8458^2834  +  a^SjSjSj^  -  S6SJS2C345  +  S7s1s6S345c2  -  a7gs1s2(s7S345  -  c6c7S345)), 

UV32    =  -  a45C2S34  -  a56C2S345    +    S6C2C345  -  S7S6S345C2      +    a78C2(S7C345  -  C^Sjjs)), 

uv42  =  0,       uv52  -  0,       uva  =  0, 
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uv13  =     -  a^CjCjC^  -  ^SjJ  -  axicfi&B  -  s^)  "  ^(c^s^s  +  sic345)  -  S7(c1s6%15c2  - 

S1S6S345)  "  ^(Cl^taSMS    +    C6C7C345)    +    Sl(S7C345  "  C6C7S34s))' 

uv23  =     -  a45(s1c2c34  +  c^)  -  aM(s1c2c3tf  +  c^)  -  Sfa&h*  -  cic34s)  -  S7(s1s6C345C2  + 

C1S6S345)   "  ^(Si^CStS^    +    GfifiMSt   '  Cl(S7C345  "  C6C7S34s))> 
UV33    =  "  a45S2C34  "  a56S2C345  "  S^Sj^  -  8784034582     "   37382(878345    +    C6C7C345)), 

UV43  =  0,         uv53  =   0,         uv63  =  0, 

uv14  =     -  345(0^2034  -  s^)  -  a56(c1c2c345  -  s1s34S)  -  S6(c1c2s345  +  s^)  -  S7(c1s6C345C2  - 

SlS6S34s)   "  a78(ClC2VS7S345    +    C6C7C345)    +    Sl(S7C345  "  C&P&S/h 

uv24  =     -  345(8^2034  +  0^34)  -  a56(s1c2C345  +  Cj&xs)  -  S6(s1c2s345  -  c^)  -  S7(s1s6C345C2  + 

C1S6S345)   "  a78(SlC2(S7S345    +    C6C7C345/   "  Cl(S7C345  "  C6C7S345))> 
UV34    =  "  ^UjSjC^  "  a56S2C345  -  S6S2S345  -   S7S6C345S2     "   37382(878345    +    ^C^s)), 

UV44    =    0,  UV^    =    0,  UVM    =    0, 

UVtf    =  "  356(0^20345  -  SjS^)   -  S6(C1C2S345    +    S^j)  -  S7(C1S6C345C2  -  SjS^)  -  378(C1C2(S7S345 

+    C6C7C345)    +    S1(S7C345  -  C6C7S345)), 

UV25  =       -  a^SjCjC^  +  C1S345)  -  S6(S1C2S345  -  0^345)  -  S7(S1S6C345C2  +  C1S6S345)  -  ^(^^(StS^ 

+  (^070345)  -  Cj^c^  -  c^s^)), 


UV35  = 


a56S2C345  "  S6S2S345   -  078^34582     ■  37382(878345    +    C6C7C345;;, 


UV45  =  0,       UV55  =  0,       uvtf  =  0, 

UV16  =        -S7(C1C6C345C2   +   SJC6C34J)   +   37g(C1C2S6C7S345   +   s1s6c7C345;, 

UV26  =  "S7(S1C6C345C2  "  CjC^C^j)    +    37g(S1C2S6C7S345   -  C^S^C^), 

UVJ6  =  -07C6S345S2    +    378S2S6C7S345, 

UV«  =  CjC^s  -  8,83,5,  UV56    =    S1C2C345    +    0^345,  UV^    =    SiCj^, 
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uv17  = 


uv27  = 


uv37  = 


a78(ClC2(C7C345    +    C6S7S345/  "  Sl(C7S345  "  C6S7C345/)> 
a78(SlC2(C7C345    "*"    C6S7S345)    *    Cl(C7S345  "   C6S7C345.).)> 
a78S2(.C7C345    "*"    C6S7S345A 


UV47    -    -CjC^SgSj^  -  S^Cj^,  UV57    -    "SjCjSjS^    +    CjSjC^s, 


uv67  = 


vwn  = 


vw21  = 


vw31  = 


vwu  = 


VW^    = 


vw32  = 


vw42  = 


vw13  = 


vw. 


23 


vw 
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vw43  = 


vw14  = 


-S2S6S345, 

*9&i<&ms  -  cic345)  -  SfiCs^c^  +  CjSatf)  +  S7(s1s6s345c2  -  qs^)  -  &n(Si<h.(h<h*s 

+    CjCjSj^)    +    C1(S7S345    +    C6C7CJ45)), 

-  a56(c1c2s345  +   s^^)   +   S^qcjc^  -  s^)  -  S7(c1s6s345c2     +   SjS^)   + 

a78VClC2(S7C345  "  C6C7S34s)   "  Sl(S7S345    "*"    C6C7C34SM 

0,         vw41   =  0,         vw51   =  0,         VW6i   =  0, 

a56cis2s345 "  S6c1s2c34S  +  S7c1s6s345s2  -  a78cIs2(s7c345  -  c6c7s345)), 

a56s1s2s345  -  S6s1s2c345  +  S7s1s6s345s2  -  a7gS1s2(s7c345  -  Cj^s^j);, 

-  ajj^s^j  +  SjCjCj^  -  S7s6S345c2   +  37802(870345  -  04078345);, 
0,       v52  =  0,       v^  =  0, 

-  a«(CiC2C34j  -  SjSjJ  -  S^CjCjS^  +  s^)  -  S7(c1s6C345C2  -  SjS^)  -  a^c^^s^ 

+    C6C7C345)    +    S1(S7C345  "  C^Sjy)), 

-  a56(s1c2C345  +  0^345)  -  S^s^Sj^  -  0^345)  -  S7(s1s6C345c2  +  qs^)  -  a78(s1c2(s7s345 

+    C6C7C345)  -  C1(S7C345  -  C6C7S345)), 

-  a56S2C345  -  S6S2S345  -  S7S6C345S2    -  a7gS2(s7S345  +  ^070345); 

0,       VW53  =  0,       vw63  -  0, 

-  a56(c1C2C345  -  SjSj,,)  -  S6(C1C2S345   +   S^j)  -  S7(C1S6C345C2  -  SjS^s)  -  a78(c1C2(s7S345 

T    C^Cj^)    +    Sj^Cj^  -   ^078345);, 
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vw24  =     -  asffc&Cja  +  0,8345)  -  S6(s1c2s343  -  c^)  -  S^SAc^Cj  +  cfifad  -  avsCsiCzC^Sj^ 

+    C6C7C34j)   -  C1(S7C345  -  ^078345)), 
VW34   =        -  a56S2C345  -  S6S2S345  -  S7S6C345S2     "  a7gS2(s7s345   +    C6C7C345)), 
VW44  =  0,         VW54  =  0,         vwM  =  0, 

vwu  =     -  ZxicfrCxs  +  s1s345)  -  S6(c1c2s345  +  5,0345)  -  Srfos^Cj  -  s1s6S345)  -  a78(c1c2(s7S345 

+   C6C7C34S)   +   S1(S7C34S  -  C6C7Sj45)), 
VWjs    =        -  356(8,020345   +   C^)  -  S^C^Sja  -  C,C345)  -  Sfafifi*&  +    Wms)  "  *n(Sl<h.(Sp34S 

+    C6C7C345)   -  C,(S7C345  -   C6CtS34j)), 
^35    =         "  a56S2C345  "  S6S2S345  -  S7S6C345S2     -  a-jgSj^^s    +    CjC^Cj^)), 
VW^    =    0,  VW55    -    0,  VW^    =    0, 


vWjg  —     -b7c6S345s2  +  a7gs2s6c7S345, 

VW^    =    C1C2C345  -  S^^,  ^56    =    S,C2C345    +    C1S345,  VWW    =    S2C345, 

VWl7    =         a-T&\ClC2\CTC34S    +    C6S7S345)   "  Sl(C7S345  "  C6S7C34s))> 
VW27   =       a78(S,C2(C7C345   +    CjSjSms)    +    C,(C7S345  -  c6s7C345)), 

VW27    =         a78S2(C7C345    +    C6S7S345)> 

VW47    =    -C,C2S6S345  -  S^Cjy,  VW57    =    -5,025,55345    +    Cj&fi^s, 

VW67  =  -S2S6S345, 

uw„  =     S7(s,(-c6C345c2  +  s6s2)  -  c,c6S345)  +  a78(s,(s2c6c7  +  c^c^  +  c^c^), 
uw21  =     S7(c,(c6C345C2  -  s6s2)  -  s,c6S345)  +  a78(-c,(s2c6c7  +  c&c^  +  Sjpfyfas), 
uw3,  =     0, 


vw,6  =     -S7(c,c6C345c2  +  s,c6C345)  +  a78(c,C2S6c7S345  +  s^c^), 

- 

^26    =         "S7(SiC6C345C2  "  C,C6C345)    +    a7s(S,C2S6C7S345  -   CjS^Cj^), 
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uw41  =  0,       uw31  =  0,       uw61  =  0, 
uwu  =     -S^c^CsC^jSj  +  s^))  +  a^c^s^c^  -  CjC^)), 

UW^   =       -S7(S1(C6C345S2   +   S^))   +   bMspfifiw  -  ^CjC,)), 

uw32  =     S^c^j^Sj  -  s6s2))  -  ayg^CjSgCyC^  +  SjCjC,)), 

uw42  =  0,       uw52  =  0,       uw^  =  0, 

uw13  =     -Sv^CjCjCj^Cj  +  S^Cjtt)  +  a78(c1c2s6c7s345  +  s^CyCj^), 

UW23    =        "^7(S1C6C345C2  "  C1C6C345/    "^    a78\SlC2S6C7S345  "   C1S6C7C345.)> 
UW33    «=        -07C6S345S2    +    a7gS2S6C7S345, 

uw43  =  0,       uw53  =  0,       uw63  =  0, 

uw14  =     -S7(c1c6C345c2  +  SjCjC^)  +  a78(c1c2s6c7s345  +  SjS^c^), 

uw24  =     -S7(s1c6c345c2  -  CjC^Cj^)  +  a7g(s1c2s6c7S345  -  c1s6c7c345), 

UW34    =        "^7C6S345S2  "  a78S2S6C7^345» 

uw^,  =  0,       uwM  =  0,       uwM  =  0, 

uwu  =     -S7(c1c6C345c2  +  s1c6c345)  +  a7g(c1c2s6c7s345  +  SJS6C7CJ45), 

UW25    =        -S7(S1C6C345C2  -  C^C^)    +    a7g(S1C2S6C7S345   -  C^CyCj^), 

uw35  =  -57c6S345s2  -  a7gs2s6c7s345, 

uw45  =  0,       uw55  =  0,       uw^  =  0, 

uw16  =  -S7(c1s6c345c2  -  s1s6s345  +  qc^)  +  a78(-c1c2c6c7c345  +  s1s6c7s345  +  cfrs&j), 

uw26  =  -S7(s1s6c345c2  +  CjSjS^  +  S!C6s2)  +  a78(-s1c2c6c7c345  -  c1c6c7s345  +  SjS^c,), 

uw36  =  "57(S6C345s2  -  c6c2)  -  a7g(s2s6c7c345  +  028407), 

uw^  =  0,       uw56  =  0,       uwtt  =  0, 

uw17  =  a7g(c1(s2c6s7  +  CjS^c^j)  -  s1s6s7s345), 
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uw27  =     a78(s1(s2c6s7  +  C^StC^s)  +  cj&fas), 
uw37  =     a78(-C2C6s7  +  s^s^), 

UW47  =       C1(C2C6C34S  -  S2S6)  -  SjCjS^s, 

UW57  =     S^Cj^C^  -  s2s6)  +  qc^j, 
UW67  =      S2C6C345  +  CjS* 

uvwn  =  a78(c1(c7c345  +  qs^)  +  s^c^StC^  -  c^s^  -  s2s6s7)), 

uvw21  =  alSisfoc^  +  c6s7S345)  +  c1(-c2c6s7c345  +  c^s-^  +  s2s6s7)), 

uvw31  =  0, 

uvw41  =  0,       uvw51  =  0,       uvw61  =  0, 

uvw^  =  a7gc1(c2s6s7  -  s2c7S345  +  s2c6s7C345), 

UVW^  =    a7gS1(C2S6S7  -  S2C7S345  +  s2c6s7C345), 

uvw32  =  a78(s2s6s7  +  02(0,8345  -  c6s7C345)), 

uvw42  =  0,       uvw52  =  0,       uvw62  =  0, 

uvw13  =  a78(c1c2(c7c345  +  c6s7S345)  -  s^c^  -  c6s7C345)), 

uvw^  =  a78(s1c2(c7c345  +  c6s7S345)  +  c1(c7s345  -  c6s7C345)), 

UVW33  =  a78s2(c7C345  +  C6S7S345), 

uvw43  =  0,       uvw53  =  0,       uvw63  =  0, 

uvw14  =  a78(c1c2(c7c345  +  c6s7S345)  -  s1(c7S345  -  c6s7Cj45)), 

uvw24  =  a78(s1c2(c7C345  +  c6s7S345)  +  c1(c7S345  -  06870345)), 

UVW34    =     a78S2(C7C345    +    C6S7S345), 

UVW44  =  0,       uvwj4  =  0,       uvwM  =  0, 

UVWtf    =     a78(C1C2(C7C345    +    C6S7S345)   -  S1(C7Sj45  -  CfoCus)), 
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uvw^  =  a7g(s1c2(c7c345  +  c6s7S345)  +  c^s-^  -  c^c-^)), 

uvw45  =  0,       uvw55  =  0,       uvu^  =  0, 
uvw16  =  a78(c1(s2c6s7  +  c2s6s7C345)  -  s^s^), 

UVW26  ■    Z-nfaihCfo  +   ^Wms)   +   C1S6S7S345). 

uvw^  =  a78(-c2c6s7  +  s2s6s7C345), 

uvw^  =  0,       uvw56  =  0,       uvw^  =  0, 

uvw17  =  a7g(-c1Cj(s7S343  +  c6c7C345)  -  s^SjCmj  -  qc^)  +  CiS2s6C7), 

uvw27  =  a7g(-s1c2(s7s345  +  c6c7C345)  +  c1(s7c345  -  c^s^)  +  s^c,), 

uvw37  =  a78(-s2(s7Sj45  +  c^c^)  -  C2S6c7), 
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